From 1d5da1c35eb400abb11b3907957307940523b46b Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 2 Feb 2014 22:43:00 -0500
Subject: [PATCH] Changed to Point3, which makes much more sense

---
 gtsam/navigation/GPSFactor.cpp           | 18 +++++++++---------
 gtsam/navigation/GPSFactor.h             |  4 ++--
 gtsam/navigation/tests/testGPSFactor.cpp |  4 ++--
 3 files changed, 13 insertions(+), 13 deletions(-)

diff --git a/gtsam/navigation/GPSFactor.cpp b/gtsam/navigation/GPSFactor.cpp
index c7cd43edd..fd769b188 100644
--- a/gtsam/navigation/GPSFactor.cpp
+++ b/gtsam/navigation/GPSFactor.cpp
@@ -48,26 +48,26 @@ Vector GPSFactor::evaluateError(const Pose3& p,
 }
 
 //***************************************************************************
-pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Vector3& NED1,
-    double t2, const Vector3& NED2, double timestamp) {
+pair<Pose3, Vector3> GPSFactor::EstimateState(double t1, const Point3& NED1,
+    double t2, const Point3& NED2, double timestamp) {
   // Estimate initial velocity as difference in NED frame
   double dt = t2 - t1;
-  Vector3 nV = (NED2 - NED1) / dt;
+  Point3 nV = (NED2 - NED1) / dt;
 
   // Estimate initial position as linear interpolation
-  Vector3 nT = NED1 + nV * (timestamp - t1);
+  Point3 nT = NED1 + nV * (timestamp - t1);
 
   // Estimate Rotation
-  double yaw = atan2(nV[1], nV[0]);
+  double yaw = atan2(nV.y(), nV.x());
   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;
+  Point3 yV = nRy.inverse() * nV; // velocity in yaw frame
+  double pitch = -atan2(yV.z(), yV.x()), roll = 0;
   Rot3 nRb = Rot3::ypr(yaw, pitch, roll);
 
   // Construct initial pose
-  Pose3 nTb(nRb, Point3(nT[0], nT[1], nT[2])); // nTb
+  Pose3 nTb(nRb, nT); // nTb
 
-  return make_pair(nTb, nV);
+  return make_pair(nTb, nV.vector());
 }
 //***************************************************************************
 
diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h
index cb52f25fd..f4ef6c49a 100644
--- a/gtsam/navigation/GPSFactor.h
+++ b/gtsam/navigation/GPSFactor.h
@@ -95,8 +95,8 @@ public:
    *  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);
+  static std::pair<Pose3, Vector3> EstimateState(double t1, const Point3& NED1,
+      double t2, const Point3& NED2, double timestamp);
 
 private:
 
diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp
index bade854af..f02047aa9 100644
--- a/gtsam/navigation/tests/testGPSFactor.cpp
+++ b/gtsam/navigation/tests/testGPSFactor.cpp
@@ -72,7 +72,7 @@ TEST(GPSData, init) {
 
   // GPS Reading 1 will be ENU origin
   double t1 = 84831;
-  Vector3 NED1(0, 0, 0);
+  Point3 NED1(0, 0, 0);
   LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54,
       Geocentric::WGS84);
 
@@ -80,7 +80,7 @@ TEST(GPSData, init) {
   double t2 = 84831.5;
   double E, N, U;
   enu.Forward(35.4394633333333, -119.063146666667, 276.52, E, N, U);
-  Vector3 NED2(N, E, -U);
+  Point3 NED2(N, E, -U);
 
   // Estimate initial state
   Pose3 T;