testGPSFactor.cpp
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
20 #include <gtsam/base/Testable.h>
22 
24 
25 #include <GeographicLib/Config.h>
27 
28 using namespace std;
29 using namespace gtsam;
30 using namespace GeographicLib;
31 
32 #if GEOGRAPHICLIB_VERSION_MINOR<37
33 static const auto& kWGS84 = Geocentric::WGS84;
34 #else
35 static const auto& kWGS84 = Geocentric::WGS84();
36 #endif
37 
38 // *************************************************************************
39 namespace example {
40 // ENU Origin is where the plane was in hold next to runway
41 const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
42 
43 // Convert from GPS to ENU
45 
46 // Dekalb-Peachtree Airport runway 2L
47 const double lat = 33.87071, lon = -84.30482, h = 274;
48 }
49 
50 // *************************************************************************
51 TEST( GPSFactor, Constructor ) {
52  using namespace example;
53 
54  // From lat-lon to geocentric
55  double E, N, U;
56  origin_ENU.Forward(lat, lon, h, E, N, U);
57  EXPECT_DOUBLES_EQUAL(133.24, E, 1e-2);
58  EXPECT_DOUBLES_EQUAL(80.98, N, 1e-2);
59  EXPECT_DOUBLES_EQUAL(0, U, 1e-2);
60 
61  // Factor
62  Key key(1);
63  SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
64  GPSFactor factor(key, Point3(E, N, U), model);
65 
66  // Create a linearization point at zero error
67  Pose3 T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U));
69 
70  // Calculate numerical derivatives
71  Matrix expectedH = numericalDerivative11<Vector, Pose3>(
72  [&factor](const Pose3& T) { return factor.evaluateError(T); }, T);
73 
74  // Use the factor to calculate the derivative
75  Matrix actualH;
76  factor.evaluateError(T, actualH);
77 
78  // Verify we get the expected error
79  EXPECT(assert_equal(expectedH, actualH, 1e-8));
80 }
81 
82 // *************************************************************************
83 TEST( GPSFactor2, Constructor ) {
84  using namespace example;
85 
86  // From lat-lon to geocentric
87  double E, N, U;
88  origin_ENU.Forward(lat, lon, h, E, N, U);
89 
90  // Factor
91  Key key(1);
92  SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
93  GPSFactor2 factor(key, Point3(E, N, U), model);
94 
95  // Create a linearization point at zero error
96  NavState T(Rot3::RzRyRx(0.15, -0.30, 0.45), Point3(E, N, U), Vector3::Zero());
98 
99  // Calculate numerical derivatives
100  Matrix expectedH = numericalDerivative11<Vector, NavState>(
101  [&factor](const NavState& T) { return factor.evaluateError(T); }, T);
102 
103  // Use the factor to calculate the derivative
104  Matrix actualH;
105  factor.evaluateError(T, actualH);
106 
107  // Verify we get the expected error
108  EXPECT(assert_equal(expectedH, actualH, 1e-8));
109 }
110 
111 //***************************************************************************
112 TEST(GPSData, init) {
113 
114  // GPS Reading 1 will be ENU origin
115  double t1 = 84831;
116  Point3 NED1(0, 0, 0);
117  LocalCartesian enu(35.4393283333333, -119.062986666667, 275.54, kWGS84);
118 
119  // GPS Readin 2
120  double t2 = 84831.5;
121  double E, N, U;
122  enu.Forward(35.4394633333333, -119.063146666667, 276.52, E, N, U);
123  Point3 NED2(N, E, -U);
124 
125  // Estimate initial state
126  const auto [T, nV] = GPSFactor::EstimateState(t1, NED1, t2, NED2, 84831.0796);
127 
128  // Check values values
129  EXPECT(assert_equal((Vector )Vector3(29.9575, -29.0564, -1.95993), nV, 1e-4));
130  EXPECT( assert_equal(Rot3::Ypr(-0.770131, 0.046928, 0), T.rotation(), 1e-5));
131  Point3 expectedT(2.38461, -2.31289, -0.156011);
132  EXPECT(assert_equal(expectedT, T.translation(), 1e-5));
133 }
134 
135 // *************************************************************************
136 int main() {
137  TestResult tr;
138  return TestRegistry::runAllTests(tr);
139 }
140 // *************************************************************************
TestRegistry::runAllTests
static int runAllTests(TestResult &result)
Definition: TestRegistry.cpp:27
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Testable.h
Concept check for values that can be used in unit tests.
EXPECT
#define EXPECT(condition)
Definition: Test.h:150
GeographicLib
Namespace for GeographicLib.
Definition: JacobiConformal.hpp:15
gtsam::Z_3x1
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
Definition: Vector.h:46
TestHarness.h
example::h0
const double h0
Definition: testGPSFactor.cpp:41
GeographicLib::LocalCartesian::Forward
void Forward(real lat, real lon, real h, real &x, real &y, real &z) const
Definition: LocalCartesian.hpp:104
kWGS84
static const auto & kWGS84
Definition: testGPSFactor.cpp:33
gtsam::NavState
Definition: NavState.h:34
T
Eigen::Triplet< double > T
Definition: Tutorial_sparse_example.cpp:6
gtsam::Matrix
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
gtsam::Vector3
Eigen::Vector3d Vector3
Definition: Vector.h:43
h
const double h
Definition: testSimpleHelicopter.cpp:19
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
example
Definition: testOrdering.cpp:28
gtsam::GPSFactor::evaluateError
Vector evaluateError(const Pose3 &p, OptionalMatrixType H) const override
vector of errors
Definition: GPSFactor.cpp:40
main
int main()
Definition: testGPSFactor.cpp:136
numericalDerivative.h
Some functions to compute numerical derivatives.
example::lat0
const double lat0
Definition: testGPSFactor.cpp:41
example::origin_ENU
LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84)
example::lon0
const double lon0
Definition: testGPSFactor.cpp:41
gtsam::Pose3
Definition: Pose3.h:37
gtsam::SharedNoiseModel
noiseModel::Base::shared_ptr SharedNoiseModel
Definition: NoiseModel.h:762
Eigen::Triplet< double >
model
noiseModel::Diagonal::shared_ptr model
Definition: doc/Code/Pose2SLAMExample.cpp:7
EXPECT_DOUBLES_EQUAL
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:161
TestResult
Definition: TestResult.h:26
key
const gtsam::Symbol key('X', 0)
E
DiscreteKey E(5, 2)
Config.h
gtsam
traits
Definition: SFMdata.h:40
std
Definition: BFloat16.h:88
LocalCartesian.hpp
Header for GeographicLib::LocalCartesian class.
GeographicLib::LocalCartesian
Local cartesian coordinates.
Definition: LocalCartesian.hpp:38
TEST
TEST(GPSFactor, Constructor)
Definition: testGPSFactor.cpp:51
gtsam::GPSFactor2::evaluateError
Vector evaluateError(const NavState &p, OptionalMatrixType H) const override
vector of errors
Definition: GPSFactor.cpp:82
gtsam::assert_equal
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
gtsam::Point3
Vector3 Point3
Definition: Point3.h:38
U
@ U
Definition: testDecisionTree.cpp:349
gtsam::GPSFactor2
Definition: GPSFactor.h:118
N
#define N
Definition: igam.h:9
lon
static const double lon
Definition: testGeographicLib.cpp:34
screwPose2::expectedT
Point2 expectedT(-0.0446635, 0.29552)
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
init
Definition: TutorialInplaceLU.cpp:2
GPSFactor.h
Header file for GPS factor.
gtsam::GPSFactor
Definition: GPSFactor.h:35
lat
static const double lat
Definition: testGeographicLib.cpp:34


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:40:11