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

Simplify by storing s*dir

release/4.3a0
dellaert 2014-02-01 10:29:03 -05:00
parent 6d16ebf68d
commit 5369a7bd17
2 changed files with 13 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
gtsam/navigation/MagFactor.h
View File

@ -31,8 +31,7 @@ namespace gtsam {
class MagFactor: public NoiseModelFactor1<Rot2> { class MagFactor: public NoiseModelFactor1<Rot2> {
const Point3 measured_; ///< The measured magnetometer values const Point3 measured_; ///< The measured magnetometer values
const double scale_; ///< Scale factor from direction to magnetometer readings const Point3 nM_; ///< Local magnetic field (mag output units)
const Sphere2 direction_; ///< Local magnetic field direction
const Point3 bias_; ///< bias const Point3 bias_; ///< bias
public: public:
@ -42,7 +41,7 @@ public:
const Sphere2& direction, const Point3& bias, const Sphere2& direction, const Point3& bias,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
NoiseModelFactor1<Rot2>(model, key), // NoiseModelFactor1<Rot2>(model, key), //
measured_(measured), scale_(scale), direction_(direction), bias_(bias) { measured_(measured), nM_(scale * direction), bias_(bias) {
} }
/// @return a deep copy of this factor /// @return a deep copy of this factor
@ -51,15 +50,10 @@ public:
NonlinearFactor::shared_ptr(new MagFactor(*this))); NonlinearFactor::shared_ptr(new MagFactor(*this)));
} }
static Sphere2 unrotate(const Rot2& R, const Sphere2& p, static Point3 unrotate(const Rot2& R, const Point3& p,
boost::optional<Matrix&> HR = boost::none) { boost::optional<Matrix&> HR = boost::none) {
Sphere2 q = Rot3::yaw(R.theta()) * p; Point3 q = Rot3::yaw(R.theta()).rotate(p,HR);
if (HR) { if (HR) *HR = HR->col(2);
HR->resize(2, 1);
Point3 Q = q.point3();
Matrix B = q.basis().transpose();
(*HR) = Q.x() * B.col(1) - Q.y() * B.col(0);
}
return q; return q;
} }
@ -68,14 +62,8 @@ public:
*/ */
Vector evaluateError(const Rot2& nRb, Vector evaluateError(const Rot2& nRb,
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const {
// measured bM = nRbÕ * nM + b, where b is unknown bias // measured bM = nRbÕ * nM + b
Sphere2 rotated = unrotate(nRb, direction_, H); Point3 hx = unrotate(nRb, nM_, H) + bias_;
Point3 hx = scale_ * rotated.point3() + bias_;
if (H) {
Matrix U;
rotated.point3(U);
*H = scale_ * U * (*H);
}
return (hx - measured_).vector(); return (hx - measured_).vector();
} }
}; };
@ -112,9 +100,8 @@ public:
*/ */
Vector evaluateError(const Rot3& nRb, Vector evaluateError(const Rot3& nRb,
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const {
// measured bM = nRbÕ * nM + b, where b is unknown bias // measured bM = nRbÕ * nM + b
Point3 q = nRb.unrotate(nM_, H); Point3 hx = nRb.unrotate(nM_, H) + bias_;
Point3 hx = q + bias_;
return (hx - measured_).vector(); return (hx - measured_).vector();
} }
}; };

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

@ -58,10 +58,10 @@ using boost::none;
// ************************************************************************* // *************************************************************************
TEST( MagFactor, unrotate ) { TEST( MagFactor, unrotate ) {
Matrix H; Matrix H;
Sphere2 expected(0.457383, 0.00632703, 0.889247); Point3 expected(22735.5, 314.502, 44202.5);
EXPECT( assert_equal(expected, MagFactor::unrotate(theta,dir,H),1e-5)); EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1));
EXPECT( assert_equal(numericalDerivative11<Sphere2,Rot2> // EXPECT( assert_equal(numericalDerivative11<Point3,Rot2> //
(boost::bind(&MagFactor::unrotate, _1, dir, none), theta), H, 1e-7)); (boost::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6));
} }
// ************************************************************************* // *************************************************************************