diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp new file mode 100644 index 000000000..c7cd43edd --- /dev/null +++ b/gtsam/navigation/GPSFactor.cpp @@ -0,0 +1,74 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GPSFactor.cpp + * @author Frank Dellaert + * @brief Implementation file for GPS factor + * @date January 28, 2014 + **/ + +#include "GPSFactor.h" + +using namespace std; + +namespace gtsam { + +//*************************************************************************** +void GPSFactor::print(const string& s, const KeyFormatter& keyFormatter) const { + cout << s << "GPSFactor on " << keyFormatter(this->key()) << "\n"; + nT_.print(" prior mean: "); + this->noiseModel_->print(" noise model: "); +} + +//*************************************************************************** +bool GPSFactor::equals(const NonlinearFactor& expected, double tol) const { + const This* e = dynamic_cast(&expected); + return e != NULL && Base::equals(*e, tol) && this->nT_.equals(e->nT_, tol); +} + +//*************************************************************************** +Vector GPSFactor::evaluateError(const Pose3& p, + boost::optional H) const { + if (H) { + H->resize(3, 6); + H->block < 3, 3 > (0, 0) << zeros(3, 3); + H->block < 3, 3 > (0, 3) << p.rotation().matrix(); + } + // manifold equivalent of h(x)-z -> log(z,h(x)) + return nT_.localCoordinates(p.translation()); +} + +//*************************************************************************** +pair GPSFactor::EstimateState(double t1, const Vector3& NED1, + double t2, const Vector3& NED2, double timestamp) { + // Estimate initial velocity as difference in NED frame + double dt = t2 - t1; + Vector3 nV = (NED2 - NED1) / dt; + + // Estimate initial position as linear interpolation + Vector3 nT = NED1 + nV * (timestamp - t1); + + // Estimate Rotation + double yaw = atan2(nV[1], nV[0]); + Rot3 nRy = Rot3::yaw(yaw); // yaw frame + Vector3 yV = nRy.transpose() * nV; // velocity in yaw frame + double pitch = -atan2(yV[2], yV[0]), roll = 0; + Rot3 nRb = Rot3::ypr(yaw, pitch, roll); + + // Construct initial pose + Pose3 nTb(nRb, Point3(nT[0], nT[1], nT[2])); // nTb + + return make_pair(nTb, nV); +} +//*************************************************************************** + +}/// namespace gtsam diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 2c51bc017..9fea29ade 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -23,7 +23,12 @@ namespace gtsam { /** - * Prior on position in a local North-East-Down () navigation frame + * Prior on position in a cartesian frame. + * Possibilities include: + * ENU: East-North-Up navigation frame at some local origin + * NED: North-East-Down navigation frame at some local origin + * ECEF: Earth-centered Earth-fixed, origin at Earth's center + * See Farrell08book or e.g. http://www.dirsig.org/docs/new/coordinates.html * @addtogroup Navigation */ class GPSFactor: public NoiseModelFactor1 { @@ -70,37 +75,29 @@ public: /** print */ virtual void print(const std::string& s, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { - std::cout << s << "GPSFactor on " << keyFormatter(this->key()) << "\n"; - nT_.print(" prior mean: "); - this->noiseModel_->print(" noise model: "); - } + DefaultKeyFormatter) const; /** equals */ - virtual bool equals(const NonlinearFactor& expected, - double tol = 1e-9) const { - const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && this->nT_.equals(e->nT_, tol); - } + virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; /** implement functions needed to derive from Factor */ /** vector of errors */ Vector evaluateError(const Pose3& p, - boost::optional H = boost::none) const { - if (H) { - H->resize(3, 6); - H->block < 3, 3 > (0, 0) << zeros(3, 3); - H->block < 3, 3 > (0, 3) << p.rotation().matrix(); - } - // manifold equivalent of h(x)-z -> log(z,h(x)) - return nT_.localCoordinates(p.translation()); - } + boost::optional H = boost::none) const; - const Point3 & measurementIn() const { + inline const Point3 & measurementIn() const { return nT_; } + /* + * Convenience funcion 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 EstimateState(double t1, const Vector3& NED1, + double t2, const Vector3& NED2, double timestamp); + private: /** Serialization function */ diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index c9c6fe233..7acb0b82b 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -51,20 +51,47 @@ TEST( GPSFactor, Constructors ) { GPSFactor factor(key, Point3(E, N, U), model); // Create a linearization point at the zero-error point - Pose3 pose(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11( - boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), pose); + boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); // Use the factor to calculate the derivative Matrix actualH; - factor.evaluateError(pose, actualH); + factor.evaluateError(T, actualH); // Verify we get the expected error CHECK(assert_equal(expectedH, actualH, 1e-8)); } +//*************************************************************************** +TEST(GPSData, init) { + + // GPS Reading 1 will be ENU origin + double t1 = 84831; + Vector3 NED1(0, 0, 0); + LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54, + Geocentric::WGS84); + + // GPS Readin 2 + double t2 = 84831.5; + double E, N, U; + enu.Forward(35.4394633333333, -119.063146666667, 276.52, E, N, U); + Vector3 NED2(N, E, -U); + + // Estimate initial state + Pose3 T; + Vector3 nV; + boost::tie(T, nV) = GPSFactor::EstimateState(t1, NED1, t2, NED2, 84831.0796); + + // Check values values + EXPECT(assert_equal((Vector )Vector3(29.9575, -29.0564, -1.95993), nV, 1e-4)); + EXPECT( assert_equal(Rot3::ypr(-0.770131, 0.046928, 0), T.rotation(), 1e-5)); + Point3 expectedT(2.38461, -2.31289, -0.156011); + EXPECT(assert_equal(expectedT, T.translation(), 1e-5)); +} + // ************************************************************************* int main() { TestResult tr;