From f5845374127c30b2337e42677226ca6f9959d55c Mon Sep 17 00:00:00 2001 From: Milo Knowles Date: Fri, 23 Apr 2021 10:02:41 -0400 Subject: [PATCH] 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.