From f924c21c1973c71a8d753ae92723d0631230f005 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sat, 25 Jan 2014 21:11:37 -0500
Subject: [PATCH] Added test for Jacobian

---
 gtsam/navigation/tests/testGPSFactor.cpp | 42 ++++++++++++++++++++----
 1 file changed, 36 insertions(+), 6 deletions(-)

diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp
index f6e030e4e..539482da9 100644
--- a/gtsam/navigation/tests/testGPSFactor.cpp
+++ b/gtsam/navigation/tests/testGPSFactor.cpp
@@ -17,22 +17,52 @@
  */
 
 #include <gtsam/navigation/GPSFactor.h>
+#include <gtsam/geometry/Pose3.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/numericalDerivative.h>
 #include <CppUnitLite/TestHarness.h>
+#include <GeographicLib/LocalCartesian.hpp>
 
 using namespace std;
+using namespace GeographicLib;
 using namespace gtsam;
 
 // *************************************************************************
 TEST( GPSFactor, Constructors ) {
+
+  // Convert from GPS to ENU
+  // ENU Origin is where the plane was in hold next to runway
+  const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
+  LocalCartesian enu(lat0, lon0, h0, Geocentric::WGS84);
+
+  // Dekalb-Peachtree Airport runway 2L
+  const double lat = 33.87071, lon = -84.30482, h = 274;
+
+  // From lat-lon to geocentric
+  double E, N, U;
+  enu.Forward(lat, lon, h, E, N, U);
+  EXPECT_DOUBLES_EQUAL(133.24, E, 1e-2);
+  EXPECT_DOUBLES_EQUAL(80.98, N, 1e-2);
+  EXPECT_DOUBLES_EQUAL(0, U, 1e-2);
+
+  // Factor
   Key key(1);
   SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
-  // TODO: convert from GPS to UTM
-  // GPS: -84.30482,33.87071,274
-  // UTM: 45N 250694.42 3751090.08 (Eastings, Northings)
-  // We choose ENU coordinate system with origin 250000 3751000
-  Point3 enu(694.42, 90.08, 274);
-  GPSFactor factor1(key, enu, model);
+  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));
+
+  // Calculate numerical derivatives
+  Matrix expectedH1 = numericalDerivative11<Pose3>(
+      boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), pose);
+
+  // Use the factor to calculate the derivative
+  Matrix actualH1;
+  factor.evaluateError(pose, actualH1);
+
+  // Verify we get the expected error
+  CHECK(assert_equal(expectedH1, actualH1, 1e-9));
 }
 
 // *************************************************************************