Skip to content

Commit

Permalink
adding lever arm optional argument to gps factors
Browse files Browse the repository at this point in the history
  • Loading branch information
@mnissov
mnissov committed Jan 13, 2025
1 parent 4cbf673 commit 5254b4f
Show file tree
Hide file tree
Showing 3 changed files with 55 additions and 16 deletions.
34 changes: 28 additions & 6 deletions gtsam/navigation/GPSFactor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,20 +26,31 @@ namespace gtsam {
void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << (s.empty() ? "" : s + " ") << "GPSFactor on " << keyFormatter(key())
<< "\n";
cout << " GPS measurement: " << nT_ << "\n";
cout << " GPS measurement: " << nT_.transpose() << "\n";
cout << " Lever arm: " << B_t_BG_.transpose() << "\n";
noiseModel_->print(" noise model: ");
}

//***************************************************************************
bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) && traits<Point3>::Equals(nT_, e->nT_, tol);
return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(nT_, e->nT_, tol) &&
traits<Point3>::Equals(B_t_BG_, e->B_t_BG_, tol);
}

//***************************************************************************
Vector GPSFactor::evaluateError(const Pose3& p,
OptionalMatrixType H) const {
return p.translation(H) -nT_;
const Matrix3 rot = p.rotation().matrix();
if (H) {
H->resize(3, 6);

H->block<3, 3>(0, 0) = -rot * skewSymmetric(B_t_BG_);
H->block<3, 3>(0, 3) = rot;
}

return p.translation() - (nT_ - rot * B_t_BG_);
}

//***************************************************************************
Expand Down Expand Up @@ -67,21 +78,32 @@ pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Point3& NED1,
//***************************************************************************
void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n";
cout << " GPS measurement: " << nT_.transpose() << endl;
cout << " GPS measurement: " << nT_.transpose() << "\n";
cout << " Lever arm: " << B_t_BG_.transpose() << "\n";
noiseModel_->print(" noise model: ");
}

//***************************************************************************
bool GPSFactor2::equals(const NonlinearFactor& expected, double tol) const {
const This* e = dynamic_cast<const This*>(&expected);
return e != nullptr && Base::equals(*e, tol) &&
traits<Point3>::Equals(nT_, e->nT_, tol);
traits<Point3>::Equals(nT_, e->nT_, tol) &&
traits<Point3>::Equals(B_t_BG_, e->B_t_BG_, tol);
}

//***************************************************************************
Vector GPSFactor2::evaluateError(const NavState& p,
OptionalMatrixType H) const {
return p.position(H) -nT_;
const Matrix3 rot = p.attitude().matrix();
if (H) {
H->resize(3, 9);

H->block<3, 3>(0, 0) = -rot * skewSymmetric(B_t_BG_);
H->block<3, 3>(0, 3) = rot;
H->block<3, 3>(0, 6).setZero();
}

return p.position() - (nT_ - rot * B_t_BG_);
}

//***************************************************************************
Expand Down
22 changes: 16 additions & 6 deletions gtsam/navigation/GPSFactor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
typedef NoiseModelFactorN<Pose3> Base;

Point3 nT_; ///< Position measurement in cartesian coordinates
Point3 B_t_BG_; ///< Lever arm between GPS and BODY frame

public:

Expand All @@ -52,7 +53,7 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
typedef GPSFactor This;

/** default constructor - only use for serialization */
GPSFactor(): nT_(0, 0, 0) {}
GPSFactor(): nT_(0, 0, 0), B_t_BG_(0, 0, 0) {}

~GPSFactor() override {}

Expand All @@ -63,8 +64,8 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
* @param gpsIn measurement already in correct coordinates
* @param model Gaussian noise model
*/
GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
Base(model, key), nT_(gpsIn) {
GPSFactor(Key key, const Point3& gpsIn, const SharedNoiseModel& model, const Point3& leverArm) :
Base(model, key), nT_(gpsIn), B_t_BG_(leverArm) {
}

/// @return a deep copy of this factor
Expand All @@ -87,6 +88,10 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
return nT_;
}

inline const Point3 & leverArm() const {
return B_t_BG_;
}

/**
* Convenience function to estimate state at time t, given two GPS
* readings (in local NED Cartesian frame) bracketing t
Expand Down Expand Up @@ -122,6 +127,7 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
typedef NoiseModelFactorN<NavState> Base;

Point3 nT_; ///< Position measurement in cartesian coordinates
Point3 B_t_BG_; ///< Lever arm between GPS and BODY frame

public:

Expand All @@ -135,13 +141,13 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
typedef GPSFactor2 This;

/// default constructor - only use for serialization
GPSFactor2():nT_(0, 0, 0) {}
GPSFactor2():nT_(0, 0, 0), B_t_BG_(0, 0, 0) {}

~GPSFactor2() override {}

/// Constructor from a measurement in a Cartesian frame.
GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model) :
Base(model, key), nT_(gpsIn) {
GPSFactor2(Key key, const Point3& gpsIn, const SharedNoiseModel& model, const Point3& leverArm) :
Base(model, key), nT_(gpsIn), B_t_BG_(leverArm) {
}

/// @return a deep copy of this factor
Expand All @@ -164,6 +170,10 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
return nT_;
}

inline const Point3 & leverArm() const {
return B_t_BG_;
}

private:

#if GTSAM_ENABLE_BOOST_SERIALIZATION
Expand Down
15 changes: 11 additions & 4 deletions gtsam/navigation/tests/testGPSFactor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,9 @@ LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84);

// Dekalb-Peachtree Airport runway 2L
const double lat = 33.87071, lon = -84.30482, h = 274;

// Random lever arm
const Point3 leverArm(0.1, 0.2, 0.3);
}

// *************************************************************************
Expand All @@ -61,10 +64,12 @@ TEST( GPSFactor, Constructor ) {
// Factor
Key key(1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
GPSFactor factor(key, Point3(E, N, U), model);
GPSFactor factor(key, Point3(E, N, U), model, leverArm);

// Create a linearization point at zero error
Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U));
const Rot3 rot = Rot3::RzRyRx(0.15, -0.30, 0.45);
const Point3 p = Point3(E, N, U) - rot * leverArm;
Pose3 T(rot, p);
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));

// Calculate numerical derivatives
Expand All @@ -90,10 +95,12 @@ TEST( GPSFactor2, Constructor ) {
// Factor
Key key(1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
GPSFactor2 factor(key, Point3(E, N, U), model);
GPSFactor2 factor(key, Point3(E, N, U), model, leverArm);

// Create a linearization point at zero error
NavState T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U), Vector3::Zero());
const Rot3 rot = Rot3::RzRyRx(0.15, -0.30, 0.45);
const Point3 p = Point3(E, N, U) - rot * leverArm;
NavState T(rot, p, Vector3::Zero());
EXPECT(assert_equal(Z_3x1,factor.evaluateError(T),1e-5));

// Calculate numerical derivatives
Expand Down

0 comments on commit 5254b4f

Please sign in to comment.