4 #ifndef GAMS_POSE_GEODETIC_CONV_H_ 5 #define GAMS_POSE_GEODETIC_CONV_H_ 11 namespace gams {
namespace pose {
namespace geodetic_util {
85 using Matrix3 = Eigen::Matrix<double, 3, 3>;
94 const double longitude,
95 const double altitude) :
114 void getReference(
double* latitude,
double* longitude,
double* altitude)
const 122 const double latitude,
const double longitude,
const double altitude,
123 double* x,
double* y,
double* z)
127 double lat_rad =
deg2Rad(latitude);
128 double lon_rad =
deg2Rad(longitude);
129 double xi = sqrt(1 - kFirstEccentricitySquared *
130 sin(lat_rad) * sin(lat_rad));
131 *x = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * cos(lon_rad);
132 *y = (kSemimajorAxis / xi + altitude) * cos(lat_rad) * sin(lon_rad);
133 *z = (kSemimajorAxis / xi *
139 const double x,
const double y,
const double z,
140 double* latitude,
double* longitude,
double* altitude)
147 double r = sqrt(x * x + y * y);
148 double Esq = kSemimajorAxis * kSemimajorAxis -
150 double F = 54 * kSemiminorAxis * kSemiminorAxis * z * z;
152 kFirstEccentricitySquared * Esq;
153 double C = (kFirstEccentricitySquared * kFirstEccentricitySquared *
154 F * r * r) / pow(G, 3);
155 double S = cbrt(1 + C + sqrt(C * C + 2 * C));
156 double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
157 double Q = sqrt(1 + 2 * kFirstEccentricitySquared *
158 kFirstEccentricitySquared * P);
159 double r_0 = -(P * kFirstEccentricitySquared * r) / (1 + Q)
160 + sqrt(0.5 * kSemimajorAxis * kSemimajorAxis * (1 + 1.0 / Q) -
161 P * (1 - kFirstEccentricitySquared) * z * z /
162 (Q * (1 + Q)) - 0.5 * P * r * r);
163 double U = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) + z * z);
164 double V = sqrt(pow((r - kFirstEccentricitySquared * r_0), 2) +
165 (1 - kFirstEccentricitySquared) * z * z);
166 double Z_0 = kSemiminorAxis * kSemiminorAxis * z / (kSemimajorAxis * V);
167 *altitude = U * (1 - kSemiminorAxis * kSemiminorAxis / (kSemimajorAxis * V));
168 *latitude =
rad2Deg(atan((z + kSecondEccentricitySquared * Z_0) / r));
169 *longitude =
rad2Deg(atan2(y, x));
173 const double x,
const double y,
const double z,
174 double* north,
double* east,
double* down)
const 190 const double north,
const double east,
const double down,
191 double* x,
double* y,
double* z)
const 205 const double latitude,
const double longitude,
const double altitude,
206 double* north,
double* east,
double* down)
const 211 ecef2Ned(x, y, z, north, east, down);
215 const double north,
const double east,
const double down,
216 double* latitude,
double* longitude,
double* altitude)
const 220 ned2Ecef(north, east, down, &x, &y, &z);
225 const double latitude,
const double longitude,
const double altitude,
226 double* east,
double* north,
double* up)
const 232 double aux_north, aux_east, aux_down;
233 ecef2Ned(x, y, z, &aux_north, &aux_east, &aux_down);
241 const double east,
const double north,
const double up,
242 double* latitude,
double* longitude,
double* altitude)
const 246 const double aux_north = north;
247 const double aux_east = east;
248 const double aux_down = -up;
250 ned2Ecef(aux_north, aux_east, aux_down, &x, &y, &z);
256 const double lon_radians)
258 const double sLat = sin(lat_radians);
259 const double sLon = sin(lon_radians);
260 const double cLat = cos(lat_radians);
261 const double cLon = cos(lon_radians);
264 ret(0, 0) = -sLat * cLon;
265 ret(0, 1) = -sLat * sLon;
270 ret(2, 0) = cLat * cLon;
271 ret(2, 1) = cLat * sLon;
280 return (radians / M_PI) * 180.0;
286 return (degrees / 180.0) * M_PI;
304 #endif // GEODETIC_CONVERTER_H_ static constexpr double kSemiminorAxis
static constexpr double kSecondEccentricitySquared
static const detail::radians_t radians
Radians unit flag; see Euler constructor.
static double deg2Rad(const double degrees)
Matrix3 ned_to_ecef_matrix_
void ned2Ecef(const double north, const double east, const double down, double *x, double *y, double *z) const
static const detail::degrees_t degrees
Degres unit flag; see Euler constructor.
void getReference(double *latitude, double *longitude, double *altitude) const
Eigen::Matrix< double, 3, 3 > Matrix3
Minimal implementation of a Matrix for internal use.
static constexpr double kFlattening
GeodeticConverter(const double latitude, const double longitude, const double altitude)
Contains all GAMS-related tools, classes and code.
static void ecef2Geodetic(const double x, const double y, const double z, double *latitude, double *longitude, double *altitude)
static constexpr double kFirstEccentricitySquared
Matrix3 ecef_to_ned_matrix_
void geodetic2Enu(const double latitude, const double longitude, const double altitude, double *east, double *north, double *up) const
double initial_longitude_
static constexpr double kSemimajorAxis
void enu2Geodetic(const double east, const double north, const double up, double *latitude, double *longitude, double *altitude) const
void geodetic2Ned(const double latitude, const double longitude, const double altitude, double *north, double *east, double *down) const
Helper class for translating between LLA, ECEF, and NED coordinates.
void ecef2Ned(const double x, const double y, const double z, double *north, double *east, double *down) const
void ned2Geodetic(const double north, const double east, const double down, double *latitude, double *longitude, double *altitude) const
static Matrix3 nRe(const double lat_radians, const double lon_radians)
static void geodetic2Ecef(const double latitude, const double longitude, const double altitude, double *x, double *y, double *z)
static double rad2Deg(const double radians)