EstimateState

Also, some more comments, and cpp file

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<const This*>(&expected);
+  return e != NULL && Base::equals(*e, tol) && this->nT_.equals(e->nT_, tol);
+}
+
+//***************************************************************************
+Vector GPSFactor::evaluateError(const Pose3& p,
+    boost::optional<Matrix&> 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<Pose3, Vector3> 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

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<Pose3> {
@@ -70,37 +75,29 @@ public:
 
   /** print */
   virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
-      DefaultKeyFormatter) const {
+      DefaultKeyFormatter) const;
-    std::cout << s << "GPSFactor on " << keyFormatter(this->key()) << "\n";
-    nT_.print("  prior mean: ");
-    this->noiseModel_->print("  noise model: ");
-  }
 
   /** equals */
-  virtual bool equals(const NonlinearFactor& expected,
+  virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const;
-      double tol = 1e-9) const {
-    const This* e = dynamic_cast<const This*>(&expected);
-    return e != NULL && Base::equals(*e, tol) && this->nT_.equals(e->nT_, tol);
-  }
 
   /** implement functions needed to derive from Factor */
 
   /** vector of errors */
   Vector evaluateError(const Pose3& p,
-      boost::optional<Matrix&> H = boost::none) const {
+      boost::optional<Matrix&> 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());
-  }
 
-  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<Pose3, Vector3> EstimateState(double t1, const Vector3& NED1,
+      double t2, const Vector3& NED2, double timestamp);
+
 private:
 
   /** Serialization function */

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<Pose3>(
-      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;