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

Compute error in the body frame and fix print()

release/4.3a0
Milo Knowles 2021-04-23 09:42:07 -04:00
parent 0313a56734
commit 378b379e56
2 changed files with 65 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
gtsam_unstable/slam/MagPoseFactor.h
View File

@ -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 POSE>
class MagPoseFactor: public NoiseModelFactor1<POSE> {
@ -29,9 +30,9 @@ class MagPoseFactor: public NoiseModelFactor1<POSE> {
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<POSE> 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<POSE> {
/**
* @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<POSE> {
const SharedNoiseModel& model,
const boost::optional<POSE>& 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<POSE> {
/** 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<POSE> {
/** Return the factor's error h(x) - z, and the optional Jacobian. */
Vector evaluateError(const POSE& nPb, boost::optional<Matrix&> 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_);

78
gtsam_unstable/slam/tests/testMagPoseFactor.cpp
View File

@ -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<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);
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();
}
// *****************************************************************************
@ -67,18 +73,10 @@ TEST(MagPoseFactor, JacobianPose2) {
Matrix H2;
// Error should be zero at the groundtruth pose.
MagPoseFactor<Pose2> 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<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, &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<Pose2> f2b = MagPoseFactor<Pose2>(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<Vector, Pose2> //
(boost::bind(&MagPoseFactor<Pose2>::evaluateError, &f2b, _1, boost::none), n_P2_b), H2, 1e-7));
(boost::bind(&MagPoseFactor<Pose2>::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<Pose3> 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<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, &f3a, _1, boost::none), n_P3_b), H3, 1e-7));
(boost::bind(&MagPoseFactor<Pose3>::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<Pose3> f3b = MagPoseFactor<Pose3>(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<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, &f3b, _1, boost::none), n_P3_b), H3, 1e-7));
(boost::bind(&MagPoseFactor<Pose3>::evaluateError, &f, _1, boost::none), n_P3_b), H3, 1e-7));
}
// *****************************************************************************