PositionWGS84.cpp
Go to the documentation of this file.
1 // PositionWGS84.cpp
2 //
3 //
4 #include <cmath>
5 
6 #include "PositionWGS84.hpp"
7 #include "../tools/MathToolbox.hpp"
8 #include <sstream>
9 #include <iomanip> // for setprecision
10 #include "../tools/toolbox.hpp"
11 #include "../tools/errorhandler.hpp"
12 
13 namespace datatypes
14 {
15 
24 {
26 }
27 
28 
30 {
31 }
32 
33 
35 {
36  double x, y, dist;
37  transformToTangentialPlane(pos, &x, &y);
38  dist = hypot(x, y);
39  return dist;
40 }
41 
42 
43 bool PositionWGS84::operator==(const PositionWGS84& other) const
44 {
45  return
47  && m_sourceId == other.m_sourceId
48  && m_latitude == other.m_latitude
50  && m_longitude == other.m_longitude
52  && m_altitudeMSL == other.m_altitudeMSL
54  && m_courseAngle == other.m_courseAngle
56  && m_yawAngle == other.m_yawAngle
58  && m_pitchAngle == other.m_pitchAngle
60  && m_rollAngle == other.m_rollAngle
62  && m_source == other.m_source
63  ;
64 }
65 
72 {
73  m_sourceId = 0;
88  m_source = Unknown;
89 }
90 
91 void PositionWGS84::setLatitudeInRad(double new_latitude)
92 {
93  m_latitude = new_latitude;
94 
95  if ((new_latitude < -M_PI_2) || (new_latitude > M_PI_2)) // note: M_PI_2 = M_PI / 2
96  {
97  printWarning("PositionWGS84::setLatitude: The given latitude (" +
98  ::toString(new_latitude * rad2deg, 6) +
99  " deg.) is outside the definition range for this value which is [-90,90]. " +
100  "Ignoring this error condition here - hopefully this value isn't used anywhere.");
101  }
102 }
103 
104 void PositionWGS84::setLongitudeInRad(double new_longitude)
105 {
106  m_longitude = new_longitude;
107 
108  if ((new_longitude < -M_PI) || (new_longitude > M_PI))
109  {
110  printWarning("PositionWGS84::setLongitude: The given longitude (" +
111  ::toString(new_longitude * rad2deg, 6) +
112  " deg.) is outside the definition range for this value which is [-180,180]. " +
113  "Ignoring this error condition here - hopefully this value isn't used anywhere.");
114  }
115 }
116 
123 double PositionWGS84::NMEA2rad( double Dm )
124 {
125  // Check this website for more information: http://home.online.no/~sigurdhu/Deg_formats.htm
126  INT16 D = static_cast<INT16>(Dm / 100.0);
127  double m = Dm - 100.0 * D;
128  return (D + m / 60.0) * deg2rad;
129 }
130 
148 void PositionWGS84::setLatitudeInNMEA( double Dm, char H )
149 {
150  if ( H == 'S' || H == 's' )
151  {
152  setLatitudeInRad(-NMEA2rad( Dm ));
153  }
154  else
155  {
156  setLatitudeInRad( NMEA2rad( Dm ));
157  }
158 }
159 
177 void PositionWGS84::setLongitudeInNMEA( double Dm, char H )
178 {
179  if ( H == 'W' || H == 'w' )
180  {
181  setLongitudeInRad(-NMEA2rad( Dm ));
182  }
183  else
184  {
185  setLongitudeInRad( NMEA2rad( Dm ));
186  }
187 }
188 
189 
195 std::string PositionWGS84::toString() const
196 {
197  std::stringstream out;
198  out << "[PositionWGS84(lat:"
199  << std::setprecision(12) << rad2deg * m_latitude
200  << " deg; lon:"
201  << std::setprecision(12) << rad2deg * m_longitude
202  << " deg; hdg:"
204  << " deg; alt:"
205  << m_altitudeMSL
206  << " m; yaw:"
207  << rad2deg * m_yawAngle
208  << " deg; pitch:"
209  << rad2deg * m_pitchAngle
210  << " deg; roll:"
211  << rad2deg * m_rollAngle
212  << " deg; source:"
213  << ::toString(m_source)
214  << ")]";
215  return out.str();
216 }
217 
224 {
226 }
227 
228 double PositionWGS84::dist( const PositionWGS84& pos ) const
229 {
230  return 6378388 * acos(sin(m_latitude) * sin(pos.getLatitudeInRad()) + cos(m_latitude) * cos(pos.getLatitudeInRad()) * cos(pos.getLongitudeInRad() - m_longitude));
231 }
232 
233 
243 //void PositionWGS84::transformToTangentialPlane(double dLat, double dLong, double dOriginLat, double dOriginLong, double* pdX, double* pdY)
244 void PositionWGS84::transformToTangentialPlane(const PositionWGS84& origin, double* easting, double* northing) const
245 {
246  double height;
247  transformToTangentialPlane(origin, easting, northing, &height);
248 }
249 
250 void PositionWGS84::transformToTangentialPlane(const PositionWGS84& origin, double* easting, double* northing, double* height) const
251 {
252  if (isNaN(getLatitudeInRad()) ||
255  {
256  printWarning("transformToTangentPlan(): At least one of the values latitude, longitude or altitude is NaN. Can't perform transformation! Returning NaN values!");
257  *easting = *northing = *height = NaN_double;
258  return;
259  }
260 
261  if (isNaN(origin.getLatitudeInRad()) ||
262  isNaN(origin.getLongitudeInRad()) ||
263  isNaN(origin. getAltitudeInMeterMSL()))
264  {
265  printWarning("transformToTangentPlan(): At least one of the origin's values latitude, longitude or altitude is NaN. Can't perform transformation! Returning NaN values!");
266  *easting = *northing = *height = NaN_double;
267  return;
268  }
269 
270  // Conversion from WGS84 to ENU (East, North, Up)
271 
272  // WGS84 Konstanten
273  const double dWGS84_SemiMajorAxis = 6378137.0;
274  const double dWGS84_SemiMinorAxis = 6356752.3142;
275  const double dWGS84_Flatness = (dWGS84_SemiMajorAxis - dWGS84_SemiMinorAxis) / dWGS84_SemiMajorAxis;
276  const double dWGS84_Eccentricity2 = dWGS84_Flatness * (2.0 - dWGS84_Flatness); // Eccentricity squared
277 
278  // WGS84 --> ECEF
279  const double dLat = getLatitudeInRad();
280  const double dLong = getLongitudeInRad();
281  const double dHeight = getAltitudeInMeterMSL(); // TODO: Wrong! This should be height above ellipsoid!
282 
283  const double dN = dWGS84_SemiMajorAxis / (sqrt(1.0 - (dWGS84_Eccentricity2 * sin(dLat) * sin(dLat))));
284  const double dX_ECEF = (dN + dHeight) * cos(dLat) * cos(dLong);
285  const double dY_ECEF = (dN + dHeight) * cos(dLat) * sin(dLong);
286  const double dZ_ECEF = ((dN * (1.0 - dWGS84_Eccentricity2)) + dHeight) * sin(dLat);
287 
288  // ECEF --> TP
289 
290  // orgin of the Tangentplane in ECEF-coordinates
291  const double dOriginLat = origin.getLatitudeInRad();
292  const double dOriginLong = origin.getLongitudeInRad();
293  const double dOriginHeight = origin.getAltitudeInMeterMSL(); // TODO: Wrong! This should be height above ellipsoid!
294 
295  const double dN_0 = dWGS84_SemiMajorAxis / (sqrt(1.0 - (dWGS84_Eccentricity2 * sin(dOriginLat) * sin(dOriginLat))));
296  const double dX_0_ECEF = (dN_0 + dOriginHeight) * cos(dOriginLat) * cos(dOriginLong);
297  const double dY_0_ECEF = (dN_0 + dOriginHeight) * cos(dOriginLat) * sin(dOriginLong);
298  const double dZ_0_ECEF = ((dN_0 * (1.0 - (dWGS84_Eccentricity2))) + dOriginHeight) * sin(dOriginLat);
299 
300  // see "Geographic Coordiante Transformation and
301  // Landmark Navigation" (T.Weiss)
302  *easting = - sin(dOriginLong) * (dX_ECEF - dX_0_ECEF)
303  + cos(dOriginLong) * (dY_ECEF - dY_0_ECEF);
304 
305  *northing = - sin(dOriginLat) * cos(dOriginLong) * (dX_ECEF - dX_0_ECEF)
306  - sin(dOriginLat) * sin(dOriginLong) * (dY_ECEF - dY_0_ECEF)
307  + cos(dOriginLat) * (dZ_ECEF - dZ_0_ECEF);
308 
309  *height = cos(dOriginLat) * cos(dOriginLong) * (dX_ECEF - dX_0_ECEF)
310  + cos(dOriginLat) * sin(dOriginLong) * (dY_ECEF - dY_0_ECEF)
311  + sin(dOriginLat) * (dZ_ECEF - dZ_0_ECEF);
312 }
313 
321 //void PositionWGS84::GPS_TransformToWGS(double dX, double dY, double dOriginLat, double dOriginLong, double* pdLat, double* pdLong)
322 void PositionWGS84::transformFromTangentialPlane(double dX, double dY, const PositionWGS84& origin)
323 {
324  INT32 nNoIterationSteps;
325  double dX_ECEF;
326  double dY_ECEF;
327  double dZ_ECEF;
328  double dLat, dLong;
329  double dX_0_ECEF;
330  double dY_0_ECEF;
331  double dZ_0_ECEF;
332  double dN;
333  double dPhi, dLambda;
334  double dP;
335  INT32 nCount;
336 
337 
338  // Konstanten setzen
339  const double dGps_SemiMajorAxis = 6378137.0;
340  const double dGps_SemiMinorAxis = 6356752.3142;
341  const double dGps_Flatness = (dGps_SemiMajorAxis - dGps_SemiMinorAxis) / dGps_SemiMajorAxis;
342  const double dGps_Eccentricity2 = dGps_Flatness * (2.0 - dGps_Flatness);
343 // const double dGps_Eccentricity = sqrt(dGps_Eccentricity2);
344 
345 
346  // Winkel werden in Grad gegeben, zum rechnen brauchen wir rad.
347  const double dOriginLat = origin.getLatitudeInRad(); // (GPS_PI / 180.0);
348  const double dOriginLong = origin.getLongitudeInRad(); // (GPS_PI / 180.0);
349 
350  nNoIterationSteps = 200;
351  dX_ECEF = 0.0;
352  dY_ECEF = 0.0;
353  dZ_ECEF = 0.0;
354 
355  // Origin of the Tangentplane in ECEF-coordinates
356  dN = dGps_SemiMajorAxis /
357  (sqrt(1 - (dGps_Eccentricity2 * sin(dOriginLat) * sin(dOriginLat))));
358 
359  dX_0_ECEF = (dN + 0.0) * cos(dOriginLat) * cos(dOriginLong);
360  dY_0_ECEF = (dN + 0.0) * cos(dOriginLat) * sin(dOriginLong);
361  dZ_0_ECEF = ((dN * (1.0 - (dGps_Eccentricity2))) + 0.0) * sin(dOriginLat);
362 
363  // see "Geographic Coordiante Transformation and
364  // Landmark Navigation" (T.Weiss)
365 
366  dLambda = dOriginLat;
367  dPhi = dOriginLong;
368 
369  dX_ECEF = -cos(dPhi) * sin(dLambda) * dX + sin(dPhi) * dY + cos(dPhi) * cos(dLambda) * 0.0 + dX_0_ECEF;
370  dY_ECEF = -sin(dPhi) * sin(dLambda) * dX - cos(dPhi) * dY + sin(dPhi) * cos(dLambda) * 0.0 + dY_0_ECEF;
371  dZ_ECEF = cos(dLambda) * dX + sin(dLambda) * 0.0 + dZ_0_ECEF;
372 
373 
374  dN = dGps_SemiMajorAxis;
375  dP = sqrt(dX_ECEF * dX_ECEF + dY_ECEF * dY_ECEF);
376 
378  // transforamtion from ECEF to geodic coordinates
379  // by an iterative numeric algorithm:
380  // perform following iteration until convergence
381  dLambda = 0.0;
382  for (nCount = 0 ; nCount < nNoIterationSteps ; nCount++)
383  {
384  dLambda = atan((dZ_ECEF + dGps_Eccentricity2 * dN * sin(dLambda)) / dP);
385  dN = dGps_SemiMajorAxis / (sqrt(1.0 - (dGps_Eccentricity2 * sin(dLambda) * sin(dLambda))));
386  }
387 
388  dLong = atan2(dY_ECEF, dX_ECEF);
389  dLat = dLambda;
390 
391  resetToDefault();
392  setLongitudeInRad(dLong); // *pdLong = dLong * (180.0 / GPS_PI);
393  setLatitudeInRad(dLat); // *pdLat = dLat * (180.0 / GPS_PI);
394 }
395 
401 {
402  double x, y;
403  transformToTangentialPlane(orign, &x, &y);
404  Point3D pos(x, y, getAltitudeInMeterMSL() - orign.getAltitudeInMeterMSL());
405  return pos;
406 }
407 
409 {
410  switch (type)
411  {
413  return "GPS (SPS)";
415  return "GPS (PPS)";
417  return "GPS (SBAS)";
419  return "GPS (Omnistar VBS)";
421  return "GPS (Omnistar HP)";
423  return "GPS (GBAS)";
425  return "GPS (RTK float)";
427  return "GPS (RTK integer)";
428  case PositionWGS84::IMU:
429  return "IMU";
431  return "Landmark positioning";
433  return "Manual";
434  case PositionWGS84::CAN:
435  return "CAN";
436  default:
437  {
438  std::stringstream ret;
439  ret << "Unknown (" << type << ")";
440  return ret.str();
441  }
442  }
443 }
444 
445 } // namespace datatypes
std::string toString(const PositionWGS84::PositionWGS84SourceType &type)
const double NaN_double
Not-a-Number in double precision.
Definition: MathToolbox.cpp:13
void setLongitudeInRad(double val)
Sets the longitude value of the WGS-84 position.
This class defines a point in the three-dimensional plane.
Definition: Point3D.hpp:25
bool operator==(const PositionWGS84 &other) const
Equality predicate.
#define rad2deg
Point3D getCartesianRelPos(const PositionWGS84 &orign) const
double getLongitudeInRad() const
Returns the longitude value of the WGS-84 position.
double hypot(double x, double y, double z)
Definition: MathToolbox.cpp:19
PositionWGS84()
Constructor.
double getLatitudeInRad() const
Returns the latitude value of the WGS-84 position.
double dist(const PositionWGS84 &pos) const
Calculates the distance in [m] from this to position in argument.
void setLatitudeInNMEA(double Dm, char H)
Sets the latitude value of the WGS-84 position.
#define deg2rad
void transformToTangentialPlane(const PositionWGS84 &origin, double *easting, double *northing, double *height) const
std::string getSourceString() const
Returns a std::string with a describtion of the WGS-84 position source.
double NMEA2rad(double Dm)
Calculates the WGS84 coordinate in rad from the mixed degree-minute value usually found on NMEA compa...
double distanceToPos(const PositionWGS84 &pos) const
double getAltitudeInMeterMSL() const
Returns the altitude in meter above mean sea level.
int32_t INT32
void transformFromTangentialPlane(double dX, double dY, const PositionWGS84 &origin)
PositionWGS84SourceType m_source
void setLatitudeInRad(double val)
Sets the latitude value of the WGS-84 position.
int16_t INT16
void resetToDefault()
Resets all values of the position to their default values.
void setLongitudeInNMEA(double Dm, char H)
Sets the longitude value of the WGS-84 position.
bool isNaN(floatT x)
Checks if a floating point value is Not-a-Number (NaN)
Definition: MathToolbox.hpp:63
void printWarning(std::string message)
std::string toString() const
Returns a std::string with a describtion of the WGS-84 position.


libsick_ldmrs
Author(s): SICK AG , Martin Günther , Jochen Sprickerhof
autogenerated on Sat Jun 8 2019 17:57:33