diff --git a/gtsam/navigation/GPSFactor.h b/gtsam/navigation/GPSFactor.h index 9fea29ade..cb52f25fd 100644 --- a/gtsam/navigation/GPSFactor.h +++ b/gtsam/navigation/GPSFactor.h @@ -23,7 +23,7 @@ namespace gtsam { /** - * Prior on position in a cartesian 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 diff --git a/gtsam/navigation/tests/testGPSFactor.cpp b/gtsam/navigation/tests/testGPSFactor.cpp index 7acb0b82b..c947cc1d9 100644 --- a/gtsam/navigation/tests/testGPSFactor.cpp +++ b/gtsam/navigation/tests/testGPSFactor.cpp @@ -17,15 +17,16 @@ */ #include -#include #include #include + #include + #include using namespace std; -using namespace GeographicLib; using namespace gtsam; +using namespace GeographicLib; // ************************************************************************* TEST( GPSFactor, Constructors ) { @@ -50,8 +51,9 @@ TEST( GPSFactor, Constructors ) { SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25); GPSFactor factor(key, Point3(E, N, U), model); - // Create a linearization point at the zero-error point - Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(-5.0, 8.0, -11.0)); + // Create a linearization point at zero error + Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U)); + CHECK(assert_equal(zero(3),factor.evaluateError(T),1e-5)); // Calculate numerical derivatives Matrix expectedH = numericalDerivative11(