From 0313a56734f5fd30d2c51ed5669c0c65741c6e62 Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Thu, 22 Apr 2021 16:51:47 -0400 Subject: [PATCH 1/4] Add MagPoseFactor --- gtsam_unstable/slam/MagPoseFactor.h | 136 ++++++++++++++++++ .../slam/tests/testMagPoseFactor.cpp | 108 ++++++++++++++ 2 files changed, 244 insertions(+) create mode 100644 gtsam_unstable/slam/MagPoseFactor.h create mode 100644 gtsam_unstable/slam/tests/testMagPoseFactor.cpp diff --git a/gtsam_unstable/slam/MagPoseFactor.h b/gtsam_unstable/slam/MagPoseFactor.h new file mode 100644 index 000000000..2e5f05be2 --- /dev/null +++ b/gtsam_unstable/slam/MagPoseFactor.h @@ -0,0 +1,136 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include + +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, + * and assumes scale, direction, and the bias are known. + */ +template +class MagPoseFactor: public NoiseModelFactor1 { + private: + typedef MagPoseFactor This; + typedef NoiseModelFactor1 Base; + typedef typename POSE::Translation Point; // Could be a Vector2 or Vector3 depending on POSE. + typedef typename POSE::Rotation Rot; + + const Point measured_; ///< The measured magnetometer data. + const Point nM_; ///< Local magnetic field (in mag output units). + const Point bias_; ///< The bias vector (in mag output units). + boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame. + + static const int MeasDim = Point::RowsAtCompileTime; + static const int PoseDim = traits::dimension; + static const int RotDim = traits::dimension; + + /// Shorthand for a smart pointer to a factor. + typedef typename boost::shared_ptr> shared_ptr; + + /** Concept check by type. */ + GTSAM_CONCEPT_TESTABLE_TYPE(POSE) + GTSAM_CONCEPT_POSE_TYPE(POSE) + + public: + ~MagPoseFactor() override {} + + /** Default constructor - only use for serialization. */ + MagPoseFactor() {} + + /** + * @param pose_key of the unknown pose nav_P_body in the factor graph. + * @param measurement magnetometer reading, a 2D or 3D vector + * @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& body_P_sensor) + : Base(model, pose_key), + measured_(measured), + nM_(scale * direction.normalized()), + 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::shared_ptr(new This(*this))); + } + + /** Implement functions needed for Testable */ + + /** print */ + void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + Base::print(s, keyFormatter); + // gtsam::print(measured_, "measured"); + // gtsam::print(nM_, "nM"); + // gtsam::print(bias_, "bias"); + } + + /** equals */ + bool equals(const NonlinearFactor& expected, double tol=1e-9) const override { + const This *e = dynamic_cast (&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. */ + Vector evaluateError(const POSE& nPb, boost::optional H = boost::none) const override { + // Get rotation of the nav frame in the sensor frame. + const Rot nRs = body_P_sensor_ ? nPb.rotation() * body_P_sensor_->rotation() : nPb.rotation(); + + // Predict the measured magnetic field h(x) in the sensor frame. + Matrix H_rot = Matrix::Zero(MeasDim, RotDim); + const Point hx = nRs.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 rot0 = nPb.rotationInterval().first; + (*H).block(0, rot0, MeasDim, RotDim) = H_rot; + } + + return (hx - measured_); + } + + private: + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(ARCHIVE & ar, const unsigned int /*version*/) { + ar & boost::serialization::make_nvp("NoiseModelFactor1", + boost::serialization::base_object(*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 diff --git a/gtsam_unstable/slam/tests/testMagPoseFactor.cpp b/gtsam_unstable/slam/tests/testMagPoseFactor.cpp new file mode 100644 index 000000000..4b151b02a --- /dev/null +++ b/gtsam_unstable/slam/tests/testMagPoseFactor.cpp @@ -0,0 +1,108 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#include +#include +#include +#include +#include +#include + +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 nRb = n_P3_b.rotation(); +Rot2 theta = n_P2_b.rotation(); + +// Sensor bias (nT). +Point3 bias3(10, -10, 50); +Point2 bias2 = bias3.head(2); + +// double s(scale * nM.norm()); +Point2 dir2(nM.head(2).normalized()); +Point3 dir3(nM.normalized()); + +// Compute the measured field in the sensor frame. +Point3 measured3 = nRb.inverse() * (scale * dir3) + bias3; + +// The 2D magnetometer will measure the "NE" field components. +Point2 measured2 = theta.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, -2)); +Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(0.4, 0.1, -0.15)), Point3(-0.1, 0.2, 0.3)); + +// ***************************************************************************** +TEST(MagPoseFactor, Constructors) { + MagPoseFactor f2a(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none); + MagPoseFactor f3a(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none); + + // Try constructing with a body_P_sensor set. + MagPoseFactor f2b = MagPoseFactor(Symbol('X', 0), measured2, scale, dir2, bias2, model2, body_P2_sensor); + MagPoseFactor f3b = MagPoseFactor(Symbol('X', 0), measured3, scale, dir3, bias3, model3, body_P3_sensor); +} + +// ***************************************************************************** +TEST(MagPoseFactor, JacobianPose2) { + Matrix H2; + + // Error should be zero at the groundtruth pose. + MagPoseFactor f2a(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none); + CHECK(gtsam::assert_equal(Z_2x1, f2a.evaluateError(n_P2_b, H2), 1e-5)); + CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // + (boost::bind(&MagPoseFactor::evaluateError, &f2a, _1, boost::none), n_P2_b), H2, 1e-7)); + + // Now test with a body_P_sensor specified, which means the raw measurement + // must be rotated into the sensor frame. + MagPoseFactor f2b = MagPoseFactor(Symbol('X', 0), + body_P2_sensor.rotation().inverse() * measured2, scale, dir2, bias2, model2, body_P2_sensor); + CHECK(gtsam::assert_equal(Z_2x1, f2b.evaluateError(n_P2_b, H2), 1e-5)); + CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // + (boost::bind(&MagPoseFactor::evaluateError, &f2b, _1, boost::none), n_P2_b), H2, 1e-7)); +} + +// ***************************************************************************** +TEST(MagPoseFactor, JacobianPose3) { + Matrix H3; + + // Error should be zero at the groundtruth pose. + MagPoseFactor f3a(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none); + CHECK(gtsam::assert_equal(Z_3x1, f3a.evaluateError(n_P3_b, H3), 1e-5)); + CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // + (boost::bind(&MagPoseFactor::evaluateError, &f3a, _1, boost::none), n_P3_b), H3, 1e-7)); + + // Now test with a body_P_sensor specified, which means the raw measurement + // must be rotated into the sensor frame. + MagPoseFactor f3b = MagPoseFactor(Symbol('X', 0), + body_P3_sensor.rotation().inverse() * measured3, scale, dir3, bias3, model3, boost::none); + CHECK(gtsam::assert_equal(Z_3x1, f3b.evaluateError(n_P3_b, H3), 1e-5)); + CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // + (boost::bind(&MagPoseFactor::evaluateError, &f3b, _1, boost::none), n_P3_b), H3, 1e-7)); +} + +// ***************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +// ***************************************************************************** From 378b379e56a11c4915821fbf4a74a02b225daafd Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Fri, 23 Apr 2021 09:42:07 -0400 Subject: [PATCH 2/4] Compute error in the body frame and fix print() --- gtsam_unstable/slam/MagPoseFactor.h | 35 ++++----- .../slam/tests/testMagPoseFactor.cpp | 78 ++++++++++++------- 2 files changed, 65 insertions(+), 48 deletions(-) diff --git a/gtsam_unstable/slam/MagPoseFactor.h b/gtsam_unstable/slam/MagPoseFactor.h index 2e5f05be2..78c223d12 100644 --- a/gtsam_unstable/slam/MagPoseFactor.h +++ b/gtsam_unstable/slam/MagPoseFactor.h @@ -19,7 +19,8 @@ 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, - * and assumes scale, direction, and the bias are known. + * where bRn is the rotation of the body in the nav frame, and scale, direction, + * and bias are assumed to be known. */ template class MagPoseFactor: public NoiseModelFactor1 { @@ -29,9 +30,9 @@ class MagPoseFactor: public NoiseModelFactor1 { typedef typename POSE::Translation Point; // Could be a Vector2 or Vector3 depending on POSE. typedef typename POSE::Rotation Rot; - const Point measured_; ///< The measured magnetometer data. - const Point nM_; ///< Local magnetic field (in mag output units). - const Point bias_; ///< The bias vector (in mag output units). + 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 body_P_sensor_; ///< The pose of the sensor in the body frame. static const int MeasDim = Point::RowsAtCompileTime; @@ -53,7 +54,7 @@ class MagPoseFactor: public NoiseModelFactor1 { /** * @param pose_key of the unknown pose nav_P_body in the factor graph. - * @param measurement magnetometer reading, a 2D or 3D vector + * @param measurement magnetometer reading in the sensor frame, a 2D or 3D vector * @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) @@ -68,9 +69,9 @@ class MagPoseFactor: public NoiseModelFactor1 { const SharedNoiseModel& model, const boost::optional& body_P_sensor) : Base(model, pose_key), - measured_(measured), + measured_(body_P_sensor ? body_P_sensor->rotation() * measured : measured), nM_(scale * direction.normalized()), - bias_(bias), + bias_(body_P_sensor ? body_P_sensor->rotation() * bias : bias), body_P_sensor_(body_P_sensor) {} /// @return a deep copy of this factor. @@ -82,11 +83,11 @@ class MagPoseFactor: public NoiseModelFactor1 { /** Implement functions needed for Testable */ /** print */ - void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { + void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { Base::print(s, keyFormatter); - // gtsam::print(measured_, "measured"); - // gtsam::print(nM_, "nM"); - // gtsam::print(bias_, "bias"); + gtsam::print(Vector(nM_), "local field (nM): "); + gtsam::print(Vector(measured_), "measured field (bM): "); + gtsam::print(Vector(bias_), "magnetometer bias: "); } /** equals */ @@ -102,18 +103,16 @@ class MagPoseFactor: public NoiseModelFactor1 { /** Return the factor's error h(x) - z, and the optional Jacobian. */ Vector evaluateError(const POSE& nPb, boost::optional H = boost::none) const override { - // Get rotation of the nav frame in the sensor frame. - const Rot nRs = body_P_sensor_ ? nPb.rotation() * body_P_sensor_->rotation() : nPb.rotation(); - - // Predict the measured magnetic field h(x) in the sensor frame. + // 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 = nRs.unrotate(nM_, H_rot, boost::none) + bias_; + 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 rot0 = nPb.rotationInterval().first; - (*H).block(0, rot0, MeasDim, RotDim) = H_rot; + const size_t rot_col0 = nPb.rotationInterval().first; + (*H).block(0, rot_col0, MeasDim, RotDim) = H_rot; } return (hx - measured_); diff --git a/gtsam_unstable/slam/tests/testMagPoseFactor.cpp b/gtsam_unstable/slam/tests/testMagPoseFactor.cpp index 4b151b02a..7cfe74df2 100644 --- a/gtsam_unstable/slam/tests/testMagPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testMagPoseFactor.cpp @@ -19,6 +19,7 @@ using namespace gtsam; +// ***************************************************************************** // Magnetic field in the nav frame (NED), with units of nT. Point3 nM(22653.29982, -1956.83010, 44202.47862); @@ -28,29 +29,29 @@ 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 nRb = n_P3_b.rotation(); -Rot2 theta = n_P2_b.rotation(); +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); -// double s(scale * nM.norm()); -Point2 dir2(nM.head(2).normalized()); Point3 dir3(nM.normalized()); +Point2 dir2(nM.head(2).normalized()); // Compute the measured field in the sensor frame. -Point3 measured3 = nRb.inverse() * (scale * dir3) + bias3; +Point3 measured3 = n_R3_b.inverse() * (scale * dir3) + bias3; // The 2D magnetometer will measure the "NE" field components. -Point2 measured2 = theta.inverse() * (scale * dir2) + bias2; +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, -2)); -Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(0.4, 0.1, -0.15)), Point3(-0.1, 0.2, 0.3)); +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) { @@ -58,8 +59,13 @@ TEST(MagPoseFactor, Constructors) { MagPoseFactor f3a(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none); // Try constructing with a body_P_sensor set. - MagPoseFactor f2b = MagPoseFactor(Symbol('X', 0), measured2, scale, dir2, bias2, model2, body_P2_sensor); - MagPoseFactor f3b = MagPoseFactor(Symbol('X', 0), measured3, scale, dir3, bias3, model3, body_P3_sensor); + MagPoseFactor f2b = MagPoseFactor( + Symbol('X', 0), measured2, scale, dir2, bias2, model2, body_P2_sensor); + MagPoseFactor f3b = MagPoseFactor( + Symbol('X', 0), measured3, scale, dir3, bias3, model3, body_P3_sensor); + + f2b.print(); + f3b.print(); } // ***************************************************************************** @@ -67,18 +73,10 @@ TEST(MagPoseFactor, JacobianPose2) { Matrix H2; // Error should be zero at the groundtruth pose. - MagPoseFactor f2a(Symbol('X', 0), measured2, scale, dir2, bias2, model2, boost::none); - CHECK(gtsam::assert_equal(Z_2x1, f2a.evaluateError(n_P2_b, H2), 1e-5)); + MagPoseFactor 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 // - (boost::bind(&MagPoseFactor::evaluateError, &f2a, _1, boost::none), n_P2_b), H2, 1e-7)); - - // Now test with a body_P_sensor specified, which means the raw measurement - // must be rotated into the sensor frame. - MagPoseFactor f2b = MagPoseFactor(Symbol('X', 0), - body_P2_sensor.rotation().inverse() * measured2, scale, dir2, bias2, model2, body_P2_sensor); - CHECK(gtsam::assert_equal(Z_2x1, f2b.evaluateError(n_P2_b, H2), 1e-5)); - CHECK(gtsam::assert_equal(gtsam::numericalDerivative11 // - (boost::bind(&MagPoseFactor::evaluateError, &f2b, _1, boost::none), n_P2_b), H2, 1e-7)); + (boost::bind(&MagPoseFactor::evaluateError, &f, _1, boost::none), n_P2_b), H2, 1e-7)); } // ***************************************************************************** @@ -86,18 +84,38 @@ TEST(MagPoseFactor, JacobianPose3) { Matrix H3; // Error should be zero at the groundtruth pose. - MagPoseFactor f3a(Symbol('X', 0), measured3, scale, dir3, bias3, model3, boost::none); - CHECK(gtsam::assert_equal(Z_3x1, f3a.evaluateError(n_P3_b, H3), 1e-5)); + MagPoseFactor 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 // - (boost::bind(&MagPoseFactor::evaluateError, &f3a, _1, boost::none), n_P3_b), H3, 1e-7)); + (boost::bind(&MagPoseFactor::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7)); +} - // Now test with a body_P_sensor specified, which means the raw measurement - // must be rotated into the sensor frame. - MagPoseFactor f3b = MagPoseFactor(Symbol('X', 0), - body_P3_sensor.rotation().inverse() * measured3, scale, dir3, bias3, model3, boost::none); - CHECK(gtsam::assert_equal(Z_3x1, f3b.evaluateError(n_P3_b, H3), 1e-5)); +// ***************************************************************************** +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 f = MagPoseFactor(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 // + (boost::bind(&MagPoseFactor::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 f = MagPoseFactor(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 // - (boost::bind(&MagPoseFactor::evaluateError, &f3b, _1, boost::none), n_P3_b), H3, 1e-7)); + (boost::bind(&MagPoseFactor::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7)); } // ***************************************************************************** From f5845374127c30b2337e42677226ca6f9959d55c Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Fri, 23 Apr 2021 10:02:41 -0400 Subject: [PATCH 3/4] Improve docs --- gtsam_unstable/slam/MagPoseFactor.h | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/gtsam_unstable/slam/MagPoseFactor.h b/gtsam_unstable/slam/MagPoseFactor.h index 78c223d12..78d11f83d 100644 --- a/gtsam_unstable/slam/MagPoseFactor.h +++ b/gtsam_unstable/slam/MagPoseFactor.h @@ -20,7 +20,9 @@ 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. + * 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 MagPoseFactor: public NoiseModelFactor1 { @@ -30,9 +32,9 @@ class MagPoseFactor: public NoiseModelFactor1 { typedef typename POSE::Translation Point; // Could be a Vector2 or Vector3 depending on POSE. typedef typename POSE::Rotation Rot; - 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. + 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 body_P_sensor_; ///< The pose of the sensor in the body frame. static const int MeasDim = Point::RowsAtCompileTime; @@ -53,8 +55,9 @@ class MagPoseFactor: public NoiseModelFactor1 { MagPoseFactor() {} /** - * @param pose_key of the unknown pose nav_P_body in the factor graph. - * @param measurement magnetometer reading in the sensor frame, a 2D or 3D vector + * Construct the factor. + * @param pose_key of the unknown pose nPb in the factor graph + * @param measurement 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) @@ -101,7 +104,9 @@ class MagPoseFactor: public NoiseModelFactor1 { /** Implement functions needed to derive from Factor. */ - /** Return the factor's error h(x) - z, and the optional Jacobian. */ + /** 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 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. From 5e2af67a7443d906acc0e80eb86a7231a90c60ae Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Tue, 1 Jun 2021 16:31:20 -0400 Subject: [PATCH 4/4] Update commment syntax and replace typedef with using --- gtsam_unstable/slam/MagPoseFactor.h | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/gtsam_unstable/slam/MagPoseFactor.h b/gtsam_unstable/slam/MagPoseFactor.h index 78d11f83d..da2a54ce9 100644 --- a/gtsam_unstable/slam/MagPoseFactor.h +++ b/gtsam_unstable/slam/MagPoseFactor.h @@ -27,10 +27,10 @@ namespace gtsam { template class MagPoseFactor: public NoiseModelFactor1 { private: - typedef MagPoseFactor This; - typedef NoiseModelFactor1 Base; - typedef typename POSE::Translation Point; // Could be a Vector2 or Vector3 depending on POSE. - typedef typename POSE::Rotation Rot; + using This = MagPoseFactor; + using Base = NoiseModelFactor1; + 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. @@ -42,22 +42,22 @@ class MagPoseFactor: public NoiseModelFactor1 { static const int RotDim = traits::dimension; /// Shorthand for a smart pointer to a factor. - typedef typename boost::shared_ptr> shared_ptr; + using shared_ptr = boost::shared_ptr>; - /** Concept check by type. */ + /// Concept check by type. GTSAM_CONCEPT_TESTABLE_TYPE(POSE) GTSAM_CONCEPT_POSE_TYPE(POSE) public: ~MagPoseFactor() override {} - /** Default constructor - only use for serialization. */ + /// Default constructor - only use for serialization. MagPoseFactor() {} /** * Construct the factor. * @param pose_key of the unknown pose nPb in the factor graph - * @param measurement magnetometer reading, a Point2 or Point3 + * @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) @@ -83,9 +83,9 @@ class MagPoseFactor: public NoiseModelFactor1 { NonlinearFactor::shared_ptr(new This(*this))); } - /** Implement functions needed for Testable */ + /// Implement functions needed for Testable. - /** print */ + // 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): "); @@ -93,7 +93,7 @@ class MagPoseFactor: public NoiseModelFactor1 { gtsam::print(Vector(bias_), "magnetometer bias: "); } - /** equals */ + /// Equals function. bool equals(const NonlinearFactor& expected, double tol=1e-9) const override { const This *e = dynamic_cast (&expected); return e != nullptr && Base::equals(*e, tol) && @@ -102,9 +102,10 @@ class MagPoseFactor: public NoiseModelFactor1 { gtsam::equal_with_abs_tol(this->bias_, e->bias_, tol); } - /** Implement functions needed to derive from Factor. */ + /// Implement functions needed to derive from Factor. - /** Return the factor's error h(x) - z, and the optional Jacobian. Note that + /** + * 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 H = boost::none) const override { @@ -124,7 +125,7 @@ class MagPoseFactor: public NoiseModelFactor1 { } private: - /** Serialization function */ + /// Serialization function. friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) {