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

Added MagFactor that solves for rotation, now called 1,2,3, with respectively 1,2,3 arguments to factor.

release/4.3a0
dellaert 2014-01-30 00:58:15 -05:00
parent 2a637e77a7
commit fd6eb2b129
2 changed files with 71 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

58
gtsam/navigation/MagFactor.h
View File

@ -23,13 +23,59 @@
namespace gtsam {
/**
* Factor to estimate rotation given magnetometer reading
* This version uses model measured bM = scale * bRn * direction + bias
* and assumes scale, direction, and the bias are given
*/
class MagFactor1: public NoiseModelFactor1<Rot3> {
const Vector3 measured_; /** The measured magnetometer values */
const double scale_;
const Sphere2 direction_;
const Vector3 bias_;
public:
/** Constructor */
MagFactor1(Key key, const Vector3& measured, const LieScalar& scale,
const Sphere2& direction, const LieVector& bias,
const SharedNoiseModel& model) :
NoiseModelFactor1<Rot3>(model, key), //
measured_(measured), scale_(scale), direction_(direction), bias_(bias) {
}
/// @return a deep copy of this factor
virtual NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor1(*this)));
}
/**
* @brief vector of errors
*/
Vector evaluateError(const Rot3& nRb,
boost::optional<Matrix&> H = boost::none) const {
// measured bM = nRbÕ * nM + b, where b is unknown bias
Sphere2 rotated = nRb.unrotate(direction_, H);
Vector3 hx = scale_ * rotated.unitVector() + bias_;
if (H) // I think H2 is 2*2, but we need 3*2
{
Matrix U;
rotated.unitVector(U);
*H = scale_ * U * (*H);
}
return hx - measured_;
}
};
/**
* Factor to calibrate local Earth magnetic field as well as magnetometer bias
* This version uses model measured bM = bRn * nM + bias
* and optimizes for both nM and the bias.
* Issue with it: expresses nM in units of magnetometer
*/
class MagFactor1: public NoiseModelFactor2<LieVector, LieVector> {
class MagFactor2: public NoiseModelFactor2<LieVector, LieVector> {
Vector3 measured_; /** The measured magnetometer values */
Matrix3 bRn_; /** The assumed known rotation from nav to body */
@ -37,7 +83,7 @@ class MagFactor1: public NoiseModelFactor2<LieVector, LieVector> {
public:
/** Constructor */
MagFactor1(Key key1, Key key2, const Vector3& measured, const Rot3& nRb,
MagFactor2(Key key1, Key key2, const Vector3& measured, const Rot3& nRb,
const SharedNoiseModel& model) :
NoiseModelFactor2<LieVector, LieVector>(model, key1, key2), //
measured_(measured), bRn_(nRb.transpose()) {
@ -46,7 +92,7 @@ public:
/// @return a deep copy of this factor
virtual NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor1(*this)));
NonlinearFactor::shared_ptr(new MagFactor2(*this)));
}
/**
@ -72,7 +118,7 @@ public:
* This version uses model measured bM = scale * bRn * direction + bias
* and optimizes for both scale, direction, and the bias.
*/
class MagFactor2: public NoiseModelFactor3<LieScalar, Sphere2, LieVector> {
class MagFactor3: public NoiseModelFactor3<LieScalar, Sphere2, LieVector> {
Vector3 measured_; /** The measured magnetometer values */
Rot3 bRn_; /** The assumed known rotation from nav to body */
@ -80,7 +126,7 @@ class MagFactor2: public NoiseModelFactor3<LieScalar, Sphere2, LieVector> {
public:
/** Constructor */
MagFactor2(Key key1, Key key2, Key key3, const Vector3& measured,
MagFactor3(Key key1, Key key2, Key key3, const Vector3& measured,
const Rot3& nRb, const SharedNoiseModel& model) :
NoiseModelFactor3<LieScalar, Sphere2, LieVector>(model, key1, key2, key3), //
measured_(measured), bRn_(nRb.inverse()) {
@ -89,7 +135,7 @@ public:
/// @return a deep copy of this factor
virtual NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<NonlinearFactor>(
NonlinearFactor::shared_ptr(new MagFactor2(*this)));
NonlinearFactor::shared_ptr(new MagFactor3(*this)));
}
/**

30
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -51,33 +51,41 @@ TEST( MagFactor, Constructors ) {
Vector3 scaled = scale * nM;
Vector3 measured = scale * nRb.transpose() * nM + bias;
LieScalar s(scale * nM.norm());
Sphere2 dir(nM[0], nM[1], nM[2]);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
Matrix expectedH1, expectedH2, expectedH3;
Matrix H1, H2, H3;
// MagFactor1
MagFactor1 f1(1, 2, measured, nRb, model);
EXPECT( assert_equal(zero(3),f1.evaluateError(scaled,bias,H1,H2),1e-5));
MagFactor1 f1(1, measured, s, dir, bias, model);
EXPECT( assert_equal(zero(3),f1.evaluateError(nRb,H1),1e-5));
EXPECT( assert_equal(numericalDerivative11<Rot3> //
(boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb),//
H1, 1e-7));
// MagFactor2
MagFactor2 f2(1, 2, measured, nRb, model);
EXPECT( assert_equal(zero(3),f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT( assert_equal(numericalDerivative11<LieVector> //
(boost::bind(&MagFactor1::evaluateError, &f1, _1, bias, none, none), scaled),//
(boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),//
H1, 1e-7));
EXPECT( assert_equal(numericalDerivative11<LieVector> //
(boost::bind(&MagFactor1::evaluateError, &f1, scaled, _1, none, none), bias),//
(boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),//
H2, 1e-7));
// MagFactor2
MagFactor2 f2(1, 2, 3, measured, nRb, model);
LieScalar s(scale*nM.norm());
Sphere2 dir(nM[0], nM[1], nM[2]);
EXPECT(assert_equal(zero(3),f2.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
MagFactor3 f3(1, 2, 3, measured, nRb, model);
EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
EXPECT(assert_equal(numericalDerivative11<LieScalar> //
(boost::bind(&MagFactor2::evaluateError, &f2, _1, dir, bias, none, none, none), s),//
(boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
H1, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Sphere2> //
(boost::bind(&MagFactor2::evaluateError, &f2, s, _1, bias, none, none, none), dir),//
(boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),//
H2, 1e-7));
EXPECT(assert_equal(numericalDerivative11<LieVector> //
(boost::bind(&MagFactor2::evaluateError, &f2, s, dir, _1, none, none, none), bias),//
(boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),//
H3, 1e-7));
}