12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Improve docs

release/4.3a0
Milo Knowles 2021-04-23 10:02:41 -04:00
parent 378b379e56
commit f584537412
1 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
gtsam_unstable/slam/MagPoseFactor.h
View File

@ -20,7 +20,9 @@ namespace gtsam {
* Factor to estimate rotation of a Pose2 or Pose3 given a magnetometer reading. * Factor to estimate rotation of a Pose2 or Pose3 given a magnetometer reading.
* This version uses the measurement model bM = scale * bRn * direction + bias, * 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, * 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 POSE> template <class POSE>
class MagPoseFactor: public NoiseModelFactor1<POSE> { class MagPoseFactor: public NoiseModelFactor1<POSE> {
@ -30,9 +32,9 @@ class MagPoseFactor: public NoiseModelFactor1<POSE> {
typedef typename POSE::Translation Point; // Could be a Vector2 or Vector3 depending on POSE. typedef typename POSE::Translation Point; // Could be a Vector2 or Vector3 depending on POSE.
typedef typename POSE::Rotation Rot; typedef typename POSE::Rotation Rot;
const Point measured_; ///< The measured magnetometer data 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 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 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. boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame.
static const int MeasDim = Point::RowsAtCompileTime; static const int MeasDim = Point::RowsAtCompileTime;
@ -53,8 +55,9 @@ class MagPoseFactor: public NoiseModelFactor1<POSE> {
MagPoseFactor() {} MagPoseFactor() {}
/** /**
* @param pose_key of the unknown pose nav_P_body in the factor graph. * Construct the factor.
* @param measurement magnetometer reading in the sensor frame, a 2D or 3D vector * @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 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 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 bias of the magnetometer, modeled as purely additive (after scaling)
@ -101,7 +104,9 @@ class MagPoseFactor: public NoiseModelFactor1<POSE> {
/** 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. */ /** 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 { Vector evaluateError(const POSE& nPb, boost::optional<Matrix&> H = boost::none) const override {
// Predict the measured magnetic field h(x) in the *body* 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. // If body_P_sensor was given, bias_ will have been rotated into the body frame.