Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adding lever-arm to GPS factors #1975

Open
wants to merge 3 commits into
base: develop
Choose a base branch
from
+63 −18
Open

Adding lever-arm to GPS factors #1975

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
5254b4f
adding lever arm optional argument to gps factors
mnissov Jan 13, 2025
57b8f6b
fix GPSFactor serialization
mnissov Jan 14, 2025
7efcb46
update GPSFactor interface
mnissov Jan 14, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 28 additions & 6 deletions gtsam/navigation/GPSFactor.cpp
View file
Open in desktop
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();
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please use aRb naming convention, where R transforms b coordinates into a coordinates.

if (H) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I did not see a numerical derivative test on this Jacobian (right?) so in my book all bets are off :-)

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_);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This formula is a bit counter-intuitive to me. I would expect to see $h(x)-z$, where $h(x)$ is the measurement prediction and $z$ the measurement, in this case nT_. If I rewrite your equation that way I get

h(x) = p.position() + rot * B_t_BG_

From which I deduce that rot==nRb which matches my definition of NavState, and B_t_BG_==bL_, a lever arm in the body frame. So, I think the documentation/notation I’m looking for is

bL_ is a lever arm in the body frame, denoting the 3D position of the GPS antenna in the body frame. Because the actual location of the antenna depends on both position and attitude, providing a lever arm makes components of the attitude observable in turns.

}

//***************************************************************************
Expand Down
24 changes: 18 additions & 6 deletions gtsam/navigation/GPSFactor.h
View file
Open in desktop
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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The naming convention ‘nT_’ says that T is in the navigation frame. I think if you want to include a lever arm in this factor you need to include more documentation about the frames involved.


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 All @@ -107,6 +112,7 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
& boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(nT_);
ar & BOOST_SERIALIZATION_NVP(B_t_BG_);
}
#endif
};
Expand All @@ -122,6 +128,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 +142,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 +171,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 All @@ -176,6 +187,7 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
& boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(nT_);
ar & BOOST_SERIALIZATION_NVP(B_t_BG_);
}
#endif
};
Expand Down
8 changes: 6 additions & 2 deletions gtsam/navigation/navigation.i
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -329,7 +329,8 @@ virtual class Pose3AttitudeFactor : gtsam::NoiseModelFactor {
#include <gtsam/navigation/GPSFactor.h>
virtual class GPSFactor : gtsam::NonlinearFactor{
GPSFactor(size_t key, const gtsam::Point3& gpsIn,
const gtsam::noiseModel::Base* model);
const gtsam::noiseModel::Base* model,
const gtsam::Point3& leverArm);

// Testable
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
Expand All @@ -338,14 +339,16 @@ virtual class GPSFactor : gtsam::NonlinearFactor{

// Standard Interface
gtsam::Point3 measurementIn() const;
gtsam::Point3 leverArm() const;

// enable serialization functionality
void serialize() const;
};

virtual class GPSFactor2 : gtsam::NonlinearFactor {
GPSFactor2(size_t key, const gtsam::Point3& gpsIn,
const gtsam::noiseModel::Base* model);
const gtsam::noiseModel::Base* model,
const gtsam::Point3& leverArm);

// Testable
void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
Expand All @@ -354,6 +357,7 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor {

// Standard Interface
gtsam::Point3 measurementIn() const;
gtsam::Point3 leverArm() const;

// enable serialization functionality
void serialize() const;
Expand Down
15 changes: 11 additions & 4 deletions gtsam/navigation/tests/testGPSFactor.cpp
View file
Open in desktop
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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would not expect you to change an existing test, but rather add an additional test with a non-zero lever arm.


// 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
Loading