Skip to content

Commit

Permalink
creating GPSFactorArm and GPSFactor2Arm w/ lever arm arguments
Browse files Browse the repository at this point in the history
  • Loading branch information
@mnissov
mnissov committed Jan 15, 2025
1 parent af1e6e3 commit b815936
Show file tree
Hide file tree
Showing 3 changed files with 270 additions and 3 deletions.
60 changes: 60 additions & 0 deletions gtsam/navigation/GPSFactor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,37 @@ pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Point3& NED1,

return make_pair(nTb, nV);
}

//***************************************************************************
void GPSFactorArm::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactorArm on " << keyFormatter(key()) << "\n";
cout << " GPS measurement: " << nT_.transpose() << "\n";
cout << " Lever arm: " << bL_.transpose() << "\n";
noiseModel_->print(" noise model: ");
}

//***************************************************************************
bool GPSFactorArm::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(bL_, e->bL_, tol);
}

//***************************************************************************
Vector GPSFactorArm::evaluateError(const Pose3& p,
OptionalMatrixType H) const {
const Matrix3 nRb = p.rotation().matrix();
if (H) {
H->resize(3, 6);

H->block<3, 3>(0, 0) = -nRb * skewSymmetric(bL_);
H->block<3, 3>(0, 3) = nRb;
}

return p.translation() + nRb * bL_ - nT_;
}

//***************************************************************************
void GPSFactor2::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactor2 on " << keyFormatter(key()) << "\n";
Expand All @@ -85,5 +116,34 @@ Vector GPSFactor2::evaluateError(const NavState& p,
}

//***************************************************************************
void GPSFactor2Arm::print(const string& s, const KeyFormatter& keyFormatter) const {
cout << s << "GPSFactorArm on " << keyFormatter(key()) << "\n";
cout << " GPS measurement: " << nT_.transpose() << "\n";
cout << " Lever arm: " << bL_.transpose() << "\n";
noiseModel_->print(" noise model: ");
}

//***************************************************************************
bool GPSFactor2Arm::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(bL_, e->bL_, tol);
}

//***************************************************************************
Vector GPSFactor2Arm::evaluateError(const NavState& p,
OptionalMatrixType H) const {
const Matrix3 nRb = p.attitude().matrix();
if (H) {
H->resize(3, 9);

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

return p.position() + nRb * bL_ - nT_;
}

}/// namespace gtsam
145 changes: 143 additions & 2 deletions gtsam/navigation/GPSFactor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,10 @@
namespace gtsam {

/**
* Prior on position in a Cartesian frame.
* Prior on position in a Cartesian frame, assuming position prior is in body
* frame.
* If there exists a non-zero lever arm between body frame and GPS
* antenna, instead use GPSFactorArm.
* Possibilities include:
* ENU: East-North-Up navigation frame at some local origin
* NED: North-East-Down navigation frame at some local origin
Expand Down Expand Up @@ -112,7 +115,81 @@ class GTSAM_EXPORT GPSFactor: public NoiseModelFactorN<Pose3> {
};

/**
* Version of GPSFactor for NavState
* Version of GPSFactor (for Pose3) with lever arm between GPS and 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
* and accounts for position measurement vs state discrepancies while turning.
* @ingroup navigation
*/
class GTSAM_EXPORT GPSFactorArm: public NoiseModelFactorN<Pose3> {

private:

typedef NoiseModelFactorN<Pose3> Base;

Point3 nT_; ///< Position measurement in cartesian coordinates
Point3 bL_; ///< bL_ is a lever arm in the body frame, denoting the 3D
///< position of the GPS antenna in the body frame

public:

// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;

/// shorthand for a smart pointer to a factor
typedef std::shared_ptr<GPSFactorArm> shared_ptr;

/// Typedef to this class
typedef GPSFactorArm This;

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

~GPSFactorArm() override {}

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

/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

/// print
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;

/// equals
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

/// vector of errors
Vector evaluateError(const Pose3& p, OptionalMatrixType H) const override;

inline const Point3 & measurementIn() const {
return nT_;
}

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

/**
* Convenience function to estimate state at time t, given two GPS
* readings (in local NED Cartesian frame) bracketing t
* Assumes roll is zero, calculates yaw and pitch from NED1->NED2 vector.
*/
static std::pair<Pose3, Vector3> EstimateState(double t1, const Point3& NED1,
double t2, const Point3& NED2, double timestamp);

};

/**
* Version of GPSFactor for NavState, assuming zero lever arm between body frame
* and GPS. If there exists a non-zero lever arm between body frame and GPS
* antenna, instead use GPSFactor2Arm.
* @ingroup navigation
*/
class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
Expand Down Expand Up @@ -180,4 +257,68 @@ class GTSAM_EXPORT GPSFactor2: public NoiseModelFactorN<NavState> {
#endif
};

/**
* Version of GPSFactor2 with lever arm between GPS and 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
* and accounts for position measurement vs state discrepancies while turning.
* @ingroup navigation
*/
class GTSAM_EXPORT GPSFactor2Arm: public NoiseModelFactorN<NavState> {

private:

typedef NoiseModelFactorN<NavState> Base;

Point3 nT_; ///< Position measurement in cartesian coordinates
Point3 bL_; ///< bL_ is a lever arm in the body frame, denoting the 3D
///< position of the GPS antenna in the body frame

public:

// Provide access to the Matrix& version of evaluateError:
using Base::evaluateError;

/// shorthand for a smart pointer to a factor
typedef std::shared_ptr<GPSFactor2Arm> shared_ptr;

/// Typedef to this class
typedef GPSFactor2Arm This;

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

~GPSFactor2Arm() override {}

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

/// @return a deep copy of this factor
gtsam::NonlinearFactor::shared_ptr clone() const override {
return std::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this)));
}

/// print
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override;

/// equals
bool equals(const NonlinearFactor& expected, double tol = 1e-9) const override;

/// vector of errors
Vector evaluateError(const NavState& p, OptionalMatrixType H) const override;

inline const Point3 & measurementIn() const {
return nT_;
}

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

};

} /// namespace gtsam
68 changes: 67 additions & 1 deletion gtsam/navigation/tests/testGPSFactor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,11 @@ const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84);

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

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

}

// *************************************************************************
Expand Down Expand Up @@ -79,6 +83,37 @@ TEST( GPSFactor, Constructor ) {
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}

// *************************************************************************
TEST( GPSFactorArm, Constructor ) {
using namespace example;

// From lat-lon to geocentric
double E, N, U;
origin_ENU.Forward(lat, lon, h, E, N, U);

// Factor
Key key(1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
GPSFactorArm factor(key, Point3(E, N, U), leverArm, model);

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

// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector, Pose3>(
[&factor](const Pose3& T) { return factor.evaluateError(T); }, T);

// Use the factor to calculate the derivative
Matrix actualH;
factor.evaluateError(T, actualH);

// Verify we get the expected error
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}

// *************************************************************************
TEST( GPSFactor2, Constructor ) {
using namespace example;
Expand Down Expand Up @@ -108,6 +143,37 @@ TEST( GPSFactor2, Constructor ) {
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}

// *************************************************************************
TEST( GPSFactor2Arm, Constructor ) {
using namespace example;

// From lat-lon to geocentric
double E, N, U;
origin_ENU.Forward(lat, lon, h, E, N, U);

// Factor
Key key(1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
GPSFactor2Arm factor(key, Point3(E, N, U), leverArm, model);

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

// Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector, NavState>(
[&factor](const NavState& T) { return factor.evaluateError(T); }, T);

// Use the factor to calculate the derivative
Matrix actualH;
factor.evaluateError(T, actualH);

// Verify we get the expected error
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}

//***************************************************************************
TEST(GPSData, init) {

Expand Down

0 comments on commit b815936

Please sign in to comment.