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

Moved from Vector3/LieVector to Point3. I have mixed feelings about this. Wondering (again) whether Point3 ought to *be* a Vector3 after all.

release/4.3a0
dellaert 2014-02-01 09:31:22 -05:00
parent d555c017d6
commit 6d16ebf68d
3 changed files with 48 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/Sphere2.h
View File

@ -105,17 +105,15 @@ public:
Matrix skew() const; Matrix skew() const;
/// Return unit-norm Point3 /// Return unit-norm Point3
Point3 point3(boost::optional<Matrix&> H = boost::none) const { const Point3& point3(boost::optional<Matrix&> H = boost::none) const {
if (H) if (H)
*H = basis(); *H = basis();
return p_; return p_;
} }
/// Return unit-norm Vector /// Return scaled direction as Point3
Vector unitVector(boost::optional<Matrix&> H = boost::none) const { friend Point3 operator*(double s, const Sphere2& d) {
if (H) return s*d.p_;
*H = basis();
return (p_.vector ());
} }
/// Signed, vector-valued error between two directions /// Signed, vector-valued error between two directions

82
gtsam/navigation/MagFactor.h
View File

@ -19,7 +19,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Rot2.h> #include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/LieScalar.h> #include <gtsam/base/LieScalar.h>
namespace gtsam { namespace gtsam {
@ -31,16 +30,16 @@ namespace gtsam {
*/ */
class MagFactor: public NoiseModelFactor1<Rot2> { class MagFactor: public NoiseModelFactor1<Rot2> {
const Vector3 measured_; ///< The measured magnetometer values const Point3 measured_; ///< The measured magnetometer values
const double scale_; ///< Scale factor from direction to magnetometer readings const double scale_; ///< Scale factor from direction to magnetometer readings
const Sphere2 direction_; ///< Local magnetic field direction const Sphere2 direction_; ///< Local magnetic field direction
const Vector3 bias_; ///< bias const Point3 bias_; ///< bias
public: public:
/** Constructor */ /** Constructor */
MagFactor(Key key, const Vector3& measured, const LieScalar& scale, MagFactor(Key key, const Point3& measured, const LieScalar& scale,
const Sphere2& direction, const LieVector& 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), scale_(scale), direction_(direction), bias_(bias) {
@ -57,7 +56,7 @@ public:
Sphere2 q = Rot3::yaw(R.theta()) * p; Sphere2 q = Rot3::yaw(R.theta()) * p;
if (HR) { if (HR) {
HR->resize(2, 1); HR->resize(2, 1);
Point3 Q = q.unitVector(); Point3 Q = q.point3();
Matrix B = q.basis().transpose(); Matrix B = q.basis().transpose();
(*HR) = Q.x() * B.col(1) - Q.y() * B.col(0); (*HR) = Q.x() * B.col(1) - Q.y() * B.col(0);
} }
@ -71,13 +70,13 @@ public:
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, where b is unknown bias
Sphere2 rotated = unrotate(nRb, direction_, H); Sphere2 rotated = unrotate(nRb, direction_, H);
Vector3 hx = scale_ * rotated.unitVector() + bias_; Point3 hx = scale_ * rotated.point3() + bias_;
if (H) { if (H) {
Matrix U; Matrix U;
rotated.unitVector(U); rotated.point3(U);
*H = scale_ * U * (*H); *H = scale_ * U * (*H);
} }
return hx - measured_; return (hx - measured_).vector();
} }
}; };
@ -88,19 +87,18 @@ public:
*/ */
class MagFactor1: public NoiseModelFactor1<Rot3> { class MagFactor1: public NoiseModelFactor1<Rot3> {
const Vector3 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 Vector3 bias_; ///< bias
public: public:
/** Constructor */ /** Constructor */
MagFactor1(Key key, const Vector3& measured, const LieScalar& scale, MagFactor1(Key key, const Point3& measured, const LieScalar& scale,
const Sphere2& direction, const LieVector& bias, const Sphere2& direction, const Point3& bias,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
NoiseModelFactor1<Rot3>(model, key), // NoiseModelFactor1<Rot3>(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
@ -115,15 +113,9 @@ 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, where b is unknown bias
Sphere2 rotated = nRb.unrotate(direction_, H); Point3 q = nRb.unrotate(nM_, H);
Vector3 hx = scale_ * rotated.unitVector() + bias_; Point3 hx = q + bias_;
if (H) // I think H2 is 2*2, but we need 3*2 return (hx - measured_).vector();
{
Matrix U;
rotated.unitVector(U);
*H = scale_ * U * (*H);
}
return hx - measured_;
} }
}; };
@ -132,18 +124,18 @@ public:
* This version uses model measured bM = bRn * nM + bias * This version uses model measured bM = bRn * nM + bias
* and optimizes for both nM and the bias, where nM is in units defined by magnetometer * and optimizes for both nM and the bias, where nM is in units defined by magnetometer
*/ */
class MagFactor2: public NoiseModelFactor2<LieVector, LieVector> { class MagFactor2: public NoiseModelFactor2<Point3, Point3> {
const Vector3 measured_; ///< The measured magnetometer values const Point3 measured_; ///< The measured magnetometer values
const Matrix3 bRn_; ///< The assumed known rotation from nav to body const Rot3 bRn_; ///< The assumed known rotation from nav to body
public: public:
/** Constructor */ /** Constructor */
MagFactor2(Key key1, Key key2, const Vector3& measured, const Rot3& nRb, MagFactor2(Key key1, Key key2, const Point3& measured, const Rot3& nRb,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :
NoiseModelFactor2<LieVector, LieVector>(model, key1, key2), // NoiseModelFactor2<Point3, Point3>(model, key1, key2), //
measured_(measured), bRn_(nRb.transpose()) { measured_(measured), bRn_(nRb.inverse()) {
} }
/// @return a deep copy of this factor /// @return a deep copy of this factor
@ -157,16 +149,14 @@ public:
* @param nM (unknown) local earth magnetic field vector, in nav frame * @param nM (unknown) local earth magnetic field vector, in nav frame
* @param bias (unknown) 3D bias * @param bias (unknown) 3D bias
*/ */
Vector evaluateError(const LieVector& nM, const LieVector& bias, Vector evaluateError(const Point3& nM, const Point3& bias,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 = boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const { boost::none) const {
// measured bM = nRbÕ * nM + b, where b is unknown bias // measured bM = nRbÕ * nM + b, where b is unknown bias
Vector3 hx = bRn_ * nM + bias; Point3 hx = bRn_.rotate(nM, boost::none, H1) + bias;
if (H1)
*H1 = bRn_;
if (H2) if (H2)
*H2 = eye(3); *H2 = eye(3);
return hx - measured_; return (hx - measured_).vector();
} }
}; };
@ -175,17 +165,17 @@ public:
* This version uses model measured bM = scale * bRn * direction + bias * This version uses model measured bM = scale * bRn * direction + bias
* and optimizes for both scale, direction, and the bias. * and optimizes for both scale, direction, and the bias.
*/ */
class MagFactor3: public NoiseModelFactor3<LieScalar, Sphere2, LieVector> { class MagFactor3: public NoiseModelFactor3<LieScalar, Sphere2, Point3> {
const Vector3 measured_; ///< The measured magnetometer values const Point3 measured_; ///< The measured magnetometer values
const Rot3 bRn_; ///< The assumed known rotation from nav to body const Rot3 bRn_; ///< The assumed known rotation from nav to body
public: public:
/** Constructor */ /** Constructor */
MagFactor3(Key key1, Key key2, Key key3, const Vector3& measured, MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
const Rot3& nRb, const SharedNoiseModel& model) : const Rot3& nRb, const SharedNoiseModel& model) :
NoiseModelFactor3<LieScalar, Sphere2, LieVector>(model, key1, key2, key3), // NoiseModelFactor3<LieScalar, Sphere2, Point3>(model, key1, key2, key3), //
measured_(measured), bRn_(nRb.inverse()) { measured_(measured), bRn_(nRb.inverse()) {
} }
@ -201,23 +191,23 @@ public:
* @param bias (unknown) 3D bias * @param bias (unknown) 3D bias
*/ */
Vector evaluateError(const LieScalar& scale, const Sphere2& direction, Vector evaluateError(const LieScalar& scale, const Sphere2& direction,
const LieVector& bias, boost::optional<Matrix&> H1 = boost::none, const Point3& bias, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 = boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
boost::none) const { boost::none) const {
// measured bM = nRbÕ * nM + b, where b is unknown bias // measured bM = nRbÕ * nM + b, where b is unknown bias
Sphere2 rotated = bRn_.rotate(direction, boost::none, H2); Sphere2 rotated = bRn_.rotate(direction, boost::none, H2);
Vector3 hx = scale * rotated.unitVector() + bias; Point3 hx = scale * rotated.point3() + bias;
if (H1) if (H1)
*H1 = rotated.unitVector(); *H1 = rotated.point3().vector();
if (H2) // I think H2 is 2*2, but we need 3*2 if (H2) // H2 is 2*2, but we need 3*2
{ {
Matrix H; Matrix H;
rotated.unitVector(H); rotated.point3(H);
*H2 = scale * H * (*H2); *H2 = scale * H * (*H2);
} }
if (H3) if (H3)
*H3 = eye(3); *H3 = eye(3);
return hx - measured_; return (hx - measured_).vector();
} }
}; };

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

@ -36,20 +36,20 @@ using namespace GeographicLib;
// Get field from http://www.ngdc.noaa.gov/geomag-web/#igrfwmm // Get field from http://www.ngdc.noaa.gov/geomag-web/#igrfwmm
// Declination = -4.94 degrees (West), Inclination = 62.78 degrees Down // Declination = -4.94 degrees (West), Inclination = 62.78 degrees Down
// As NED vector, in nT: // As NED vector, in nT:
Vector3 nM(22653.29982, -1956.83010, 44202.47862); Point3 nM(22653.29982, -1956.83010, 44202.47862);
// Let's assume scale factor, // Let's assume scale factor,
double scale = 255.0 / 50000.0; double scale = 255.0 / 50000.0;
// ...ground truth orientation, // ...ground truth orientation,
Rot3 nRb = Rot3::yaw(-0.1); Rot3 nRb = Rot3::yaw(-0.1);
Rot2 theta = -nRb.yaw(); Rot2 theta = -nRb.yaw();
// ...and bias // ...and bias
Vector3 bias(10, -10, 50); Point3 bias(10, -10, 50);
// ... then we measure // ... then we measure
Vector3 scaled = scale * nM; Point3 scaled = scale * nM;
Vector3 measured = scale * nRb.transpose() * nM + bias; Point3 measured = nRb.inverse() * (scale * nM) + bias;
LieScalar s(scale * nM.norm()); LieScalar s(scale * nM.norm());
Sphere2 dir(nM[0], nM[1], nM[2]); Sphere2 dir(nM);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
@ -84,10 +84,10 @@ TEST( MagFactor, Factors ) {
// MagFactor2 // MagFactor2
MagFactor2 f2(1, 2, measured, nRb, model); MagFactor2 f2(1, 2, measured, nRb, model);
EXPECT( assert_equal(zero(3),f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT( assert_equal(zero(3),f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT( assert_equal(numericalDerivative11<LieVector> // EXPECT( assert_equal(numericalDerivative11<Point3> //
(boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),//
H1, 1e-7)); H1, 1e-7));
EXPECT( assert_equal(numericalDerivative11<LieVector> // EXPECT( assert_equal(numericalDerivative11<Point3> //
(boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// (boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),//
H2, 1e-7)); H2, 1e-7));
@ -100,7 +100,7 @@ TEST( MagFactor, Factors ) {
EXPECT(assert_equal(numericalDerivative11<Sphere2> // EXPECT(assert_equal(numericalDerivative11<Sphere2> //
(boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// (boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),//
H2, 1e-7)); H2, 1e-7));
EXPECT(assert_equal(numericalDerivative11<LieVector> // EXPECT(assert_equal(numericalDerivative11<Point3> //
(boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// (boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),//
H3, 1e-7)); H3, 1e-7));
} }