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

creating GPSFactorArm and GPSFactor2Arm w/ lever arm arguments

release/4.3a0
morten 2025-01-15 13:31:38 +01:00
parent af1e6e34e6
commit b81593601a
3 changed files with 270 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
gtsam/navigation/GPSFactor.cpp
View File

@ -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";
@ -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
gtsam/navigation/GPSFactor.h
View File

@ -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
@ -112,7 +115,81 @@ private:
};
/**
* 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> {
@ -180,4 +257,68 @@ private:
#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
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -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);
}
// *************************************************************************
@ -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;
@ -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) {