Go to the documentation of this file.
29 using namespace gtsam;
32 #if GEOGRAPHICLIB_VERSION_MINOR<37
33 static const auto&
kWGS84 = Geocentric::WGS84;
35 static const auto&
kWGS84 = Geocentric::WGS84();
41 const double lat0 = 33.86998,
lon0 = -84.30626,
h0 = 274;
47 const double lat = 33.87071,
lon = -84.30482,
h = 274;
71 Matrix expectedH = numericalDerivative11<Vector, Pose3>(
100 Matrix expectedH = numericalDerivative11<Vector, NavState>(
122 enu.
Forward(35.4394633333333, -119.063146666667, 276.52,
E,
N,
U);
126 const auto [
T, nV] = GPSFactor::EstimateState(t1, NED1, t2, NED2, 84831.0796);
static int runAllTests(TestResult &result)
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Concept check for values that can be used in unit tests.
#define EXPECT(condition)
Namespace for GeographicLib.
static const Eigen::MatrixBase< Vector3 >::ConstantReturnType Z_3x1
void Forward(real lat, real lon, real h, real &x, real &y, real &z) const
static const auto & kWGS84
Eigen::Triplet< double > T
Vector evaluateError(const Pose3 &p, OptionalMatrixType H) const override
vector of errors
Some functions to compute numerical derivatives.
LocalCartesian origin_ENU(lat0, lon0, h0, kWGS84)
noiseModel::Base::shared_ptr SharedNoiseModel
noiseModel::Diagonal::shared_ptr model
#define EXPECT_DOUBLES_EQUAL(expected, actual, threshold)
const gtsam::Symbol key('X', 0)
Header for GeographicLib::LocalCartesian class.
Local cartesian coordinates.
TEST(GPSFactor, Constructor)
Vector evaluateError(const NavState &p, OptionalMatrixType H) const override
vector of errors
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Point2 expectedT(-0.0446635, 0.29552)
std::uint64_t Key
Integer nonlinear key type.
Header file for GPS factor.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:40:11