libsick_ldmrs: PositionWGS84.cpp Source File

Go to the documentation of this file.
00001 // PositionWGS84.cpp
00002 //
00003 //
00004 #include <cmath>
00005 
00006 #include "PositionWGS84.hpp"
00007 #include "../tools/MathToolbox.hpp"
00008 #include <sstream>
00009 #include <iomanip>      // for setprecision
00010 #include "../tools/toolbox.hpp"
00011 #include "../tools/errorhandler.hpp"
00012 
00013 namespace datatypes
00014 {
00015 
00023 PositionWGS84::PositionWGS84()
00024 {
00025         resetToDefault();
00026 }
00027 
00028 
00029 PositionWGS84::~PositionWGS84()
00030 {
00031 }
00032 
00033 
00034 double PositionWGS84::distanceToPos(const PositionWGS84& pos) const
00035 {
00036         double x, y, dist;
00037         transformToTangentialPlane(pos, &x, &y);
00038         dist = hypot(x, y);
00039         return dist;
00040 }
00041 
00042 
00043 bool PositionWGS84::operator==(const PositionWGS84& other) const
00044 {
00045         return
00046                 m_timeOfMeasurement == other.m_timeOfMeasurement
00047                 && m_sourceId == other.m_sourceId
00048                 && m_latitude == other.m_latitude
00049                 && m_latitudeSigma == other.m_latitudeSigma
00050                 && m_longitude == other.m_longitude
00051                 && m_longitudeSigma == other.m_longitudeSigma
00052                 && m_altitudeMSL == other.m_altitudeMSL
00053                 && m_altitudeMSLSigma == other.m_altitudeMSLSigma
00054                 && m_courseAngle == other.m_courseAngle
00055                 && m_courseAngleSigma == other.m_courseAngleSigma
00056                 && m_yawAngle == other.m_yawAngle
00057                 && m_yawAngleSigma == other.m_yawAngleSigma
00058                 && m_pitchAngle == other.m_pitchAngle
00059                 && m_pitchAngleSigma == other.m_pitchAngleSigma
00060                 && m_rollAngle == other.m_rollAngle
00061                 && m_rollAngleSigma == other.m_rollAngleSigma
00062                 && m_source == other.m_source
00063                 ;
00064 }
00065 
00071 void PositionWGS84::resetToDefault()
00072 {
00073         m_sourceId                              = 0;
00074         m_latitude                              = NaN_double;
00075         m_latitudeSigma                 = NaN_double;
00076         m_longitude                             = NaN_double;
00077         m_longitudeSigma                = NaN_double;
00078         m_altitudeMSL                   = NaN_double;
00079         m_altitudeMSLSigma              = NaN_double;
00080         m_courseAngle                   = NaN_double;
00081         m_courseAngleSigma              = NaN_double;
00082         m_yawAngle                              = NaN_double;
00083         m_yawAngleSigma                 = NaN_double;
00084         m_pitchAngle                    = NaN_double;
00085         m_pitchAngleSigma               = NaN_double;
00086         m_rollAngle                             = NaN_double;
00087         m_rollAngleSigma                = NaN_double;
00088         m_source                                = Unknown;
00089 }
00090 
00091 void PositionWGS84::setLatitudeInRad(double new_latitude)
00092 {
00093         m_latitude = new_latitude;
00094 
00095         if ((new_latitude < -M_PI_2) || (new_latitude > M_PI_2)) // note: M_PI_2 = M_PI / 2
00096         {
00097                 printWarning("PositionWGS84::setLatitude: The given latitude (" +
00098                                                  ::toString(new_latitude * rad2deg, 6) +
00099                                                  " deg.) is outside the definition range for this value which is [-90,90]. " +
00100                                                  "Ignoring this error condition here - hopefully this value isn't used anywhere.");
00101         }
00102 }
00103 
00104 void PositionWGS84::setLongitudeInRad(double new_longitude)
00105 {
00106         m_longitude = new_longitude;
00107 
00108         if ((new_longitude < -M_PI) || (new_longitude > M_PI))
00109         {
00110                 printWarning("PositionWGS84::setLongitude: The given longitude (" +
00111                                                  ::toString(new_longitude * rad2deg, 6) +
00112                                                  " deg.) is outside the definition range for this value which is [-180,180]. " +
00113                                                  "Ignoring this error condition here - hopefully this value isn't used anywhere.");
00114         }
00115 }
00116 
00123 double PositionWGS84::NMEA2rad( double Dm )
00124 {
00125         // Check this website for more information: http://home.online.no/~sigurdhu/Deg_formats.htm
00126         INT16 D = static_cast<INT16>(Dm / 100.0);
00127         double m = Dm - 100.0 * D;
00128         return (D + m / 60.0) * deg2rad;
00129 }
00130 
00148 void PositionWGS84::setLatitudeInNMEA( double Dm, char H )
00149 {
00150         if ( H == 'S' || H == 's' )
00151         {
00152                 setLatitudeInRad(-NMEA2rad( Dm ));
00153         }
00154         else
00155         {
00156                 setLatitudeInRad( NMEA2rad( Dm ));
00157         }
00158 }
00159 
00177 void PositionWGS84::setLongitudeInNMEA( double Dm, char H )
00178 {
00179         if ( H == 'W' || H == 'w' )
00180         {
00181                 setLongitudeInRad(-NMEA2rad( Dm ));
00182         }
00183         else
00184         {
00185                 setLongitudeInRad( NMEA2rad( Dm ));
00186         }
00187 }
00188 
00189 
00195 std::string PositionWGS84::toString() const
00196 {
00197         std::stringstream out;
00198         out << "[PositionWGS84(lat:"
00199                 << std::setprecision(12) << rad2deg * m_latitude
00200                 << " deg; lon:"
00201                 << std::setprecision(12) << rad2deg * m_longitude
00202                 << " deg; hdg:"
00203                 << rad2deg * m_courseAngle
00204                 << " deg; alt:"
00205                 << m_altitudeMSL
00206                 << " m; yaw:"
00207                 << rad2deg * m_yawAngle
00208                 << " deg; pitch:"
00209                 << rad2deg * m_pitchAngle
00210                 << " deg; roll:"
00211                 << rad2deg * m_rollAngle
00212                 << " deg; source:"
00213                 << ::toString(m_source)
00214                 << ")]";
00215         return out.str();
00216 }
00217 
00223 std::string PositionWGS84::getSourceString() const
00224 {
00225         return ::toString(m_source);
00226 }
00227 
00228 double PositionWGS84::dist( const PositionWGS84& pos ) const
00229 {
00230         return 6378388 * acos(sin(m_latitude) * sin(pos.getLatitudeInRad()) + cos(m_latitude) * cos(pos.getLatitudeInRad()) * cos(pos.getLongitudeInRad() - m_longitude));
00231 }
00232 
00233 
00243 //void PositionWGS84::transformToTangentialPlane(double dLat, double dLong, double dOriginLat, double dOriginLong, double* pdX, double* pdY)
00244 void PositionWGS84::transformToTangentialPlane(const PositionWGS84& origin, double* easting, double* northing) const
00245 {
00246         double height;
00247         transformToTangentialPlane(origin, easting, northing, &height);
00248 }
00249 
00250 void PositionWGS84::transformToTangentialPlane(const PositionWGS84& origin, double* easting, double* northing, double* height) const
00251 {
00252         if (isNaN(getLatitudeInRad()) ||
00253                 isNaN(getLongitudeInRad()) ||
00254                 isNaN(getAltitudeInMeterMSL()))
00255         {
00256                 printWarning("transformToTangentPlan(): At least one of the values latitude, longitude or altitude is NaN. Can't perform transformation! Returning NaN values!");
00257                 *easting = *northing = *height = NaN_double;
00258                 return;
00259         }
00260 
00261         if (isNaN(origin.getLatitudeInRad()) ||
00262                 isNaN(origin.getLongitudeInRad()) ||
00263                 isNaN(origin. getAltitudeInMeterMSL()))
00264         {
00265                 printWarning("transformToTangentPlan(): At least one of the origin's values latitude, longitude or altitude is NaN. Can't perform transformation! Returning NaN values!");
00266                 *easting = *northing = *height = NaN_double;
00267                 return;
00268         }
00269 
00270         // Conversion from WGS84 to ENU (East, North, Up)
00271 
00272         // WGS84 Konstanten
00273         const double dWGS84_SemiMajorAxis       = 6378137.0;
00274         const double dWGS84_SemiMinorAxis       = 6356752.3142;
00275         const double dWGS84_Flatness            = (dWGS84_SemiMajorAxis - dWGS84_SemiMinorAxis) / dWGS84_SemiMajorAxis;
00276         const double dWGS84_Eccentricity2       = dWGS84_Flatness * (2.0 - dWGS84_Flatness); // Eccentricity squared
00277 
00278         // WGS84 --> ECEF
00279         const double dLat = getLatitudeInRad();
00280         const double dLong = getLongitudeInRad();
00281         const double dHeight = getAltitudeInMeterMSL();         // TODO: Wrong! This should be height above ellipsoid!
00282 
00283         const double dN = dWGS84_SemiMajorAxis / (sqrt(1.0 - (dWGS84_Eccentricity2 * sin(dLat) * sin(dLat))));
00284         const double dX_ECEF = (dN + dHeight) * cos(dLat) * cos(dLong);
00285         const double dY_ECEF = (dN + dHeight) * cos(dLat) * sin(dLong);
00286         const double dZ_ECEF = ((dN * (1.0 - dWGS84_Eccentricity2)) + dHeight) * sin(dLat);
00287 
00288         // ECEF --> TP
00289 
00290         // orgin of the Tangentplane in ECEF-coordinates
00291         const double dOriginLat = origin.getLatitudeInRad();
00292         const double dOriginLong = origin.getLongitudeInRad();
00293         const double dOriginHeight = origin.getAltitudeInMeterMSL();    // TODO: Wrong! This should be height above ellipsoid!
00294 
00295         const double dN_0 = dWGS84_SemiMajorAxis / (sqrt(1.0 - (dWGS84_Eccentricity2 * sin(dOriginLat) * sin(dOriginLat))));
00296         const double dX_0_ECEF = (dN_0 + dOriginHeight) * cos(dOriginLat) * cos(dOriginLong);
00297         const double dY_0_ECEF = (dN_0 + dOriginHeight) * cos(dOriginLat) * sin(dOriginLong);
00298         const double dZ_0_ECEF = ((dN_0 * (1.0 - (dWGS84_Eccentricity2))) + dOriginHeight) * sin(dOriginLat);
00299 
00300         // see "Geographic Coordiante Transformation and
00301         //      Landmark Navigation" (T.Weiss)
00302         *easting  = - sin(dOriginLong) * (dX_ECEF - dX_0_ECEF)
00303                                 + cos(dOriginLong) * (dY_ECEF - dY_0_ECEF);
00304 
00305         *northing = - sin(dOriginLat) * cos(dOriginLong) * (dX_ECEF - dX_0_ECEF)
00306                                 - sin(dOriginLat) * sin(dOriginLong) * (dY_ECEF - dY_0_ECEF)
00307                                 + cos(dOriginLat)                    * (dZ_ECEF - dZ_0_ECEF);
00308 
00309         *height   =   cos(dOriginLat) * cos(dOriginLong) * (dX_ECEF - dX_0_ECEF)
00310                                   + cos(dOriginLat) * sin(dOriginLong) * (dY_ECEF - dY_0_ECEF)
00311                                   + sin(dOriginLat)                    * (dZ_ECEF - dZ_0_ECEF);
00312 }
00313 
00321 //void PositionWGS84::GPS_TransformToWGS(double dX, double dY, double dOriginLat, double dOriginLong, double* pdLat, double* pdLong)
00322 void PositionWGS84::transformFromTangentialPlane(double dX, double dY, const PositionWGS84& origin)
00323 {
00324         INT32 nNoIterationSteps;
00325         double dX_ECEF;
00326         double dY_ECEF;
00327         double dZ_ECEF;
00328         double dLat, dLong;
00329         double dX_0_ECEF;
00330         double dY_0_ECEF;
00331         double dZ_0_ECEF;
00332         double dN;
00333         double dPhi, dLambda;
00334         double dP;
00335         INT32 nCount;
00336 
00337 
00338         // Konstanten setzen
00339         const double dGps_SemiMajorAxis = 6378137.0;
00340         const double dGps_SemiMinorAxis = 6356752.3142;
00341         const double dGps_Flatness              = (dGps_SemiMajorAxis - dGps_SemiMinorAxis) / dGps_SemiMajorAxis;
00342         const double dGps_Eccentricity2 = dGps_Flatness * (2.0 - dGps_Flatness);
00343 //      const double dGps_Eccentricity  = sqrt(dGps_Eccentricity2);
00344 
00345 
00346         // Winkel werden in Grad gegeben, zum rechnen brauchen wir rad.
00347         const double dOriginLat = origin.getLatitudeInRad();    // (GPS_PI / 180.0);
00348         const double dOriginLong = origin.getLongitudeInRad();  // (GPS_PI / 180.0);
00349 
00350         nNoIterationSteps = 200;
00351         dX_ECEF = 0.0;
00352         dY_ECEF = 0.0;
00353         dZ_ECEF = 0.0;
00354 
00355         // Origin of the Tangentplane in ECEF-coordinates
00356         dN = dGps_SemiMajorAxis /
00357                  (sqrt(1 - (dGps_Eccentricity2 * sin(dOriginLat) * sin(dOriginLat))));
00358 
00359         dX_0_ECEF = (dN + 0.0) * cos(dOriginLat) * cos(dOriginLong);
00360         dY_0_ECEF = (dN + 0.0) * cos(dOriginLat) * sin(dOriginLong);
00361         dZ_0_ECEF = ((dN * (1.0 - (dGps_Eccentricity2))) + 0.0) * sin(dOriginLat);
00362 
00363         // see "Geographic Coordiante Transformation and
00364         //      Landmark Navigation" (T.Weiss)
00365 
00366         dLambda = dOriginLat;
00367         dPhi    = dOriginLong;
00368 
00369         dX_ECEF = -cos(dPhi) * sin(dLambda) * dX + sin(dPhi) * dY + cos(dPhi) * cos(dLambda) * 0.0 + dX_0_ECEF;
00370         dY_ECEF = -sin(dPhi) * sin(dLambda) * dX - cos(dPhi) * dY + sin(dPhi) * cos(dLambda) * 0.0 + dY_0_ECEF;
00371         dZ_ECEF =  cos(dLambda) * dX + sin(dLambda) * 0.0 + dZ_0_ECEF;
00372 
00373 
00374         dN = dGps_SemiMajorAxis;
00375         dP = sqrt(dX_ECEF * dX_ECEF + dY_ECEF * dY_ECEF);
00376 
00378         // transforamtion from ECEF to geodic coordinates
00379         // by an iterative numeric algorithm:
00380         // perform following iteration until convergence
00381         dLambda = 0.0;
00382         for (nCount = 0 ; nCount < nNoIterationSteps ; nCount++)
00383         {
00384                 dLambda = atan((dZ_ECEF + dGps_Eccentricity2 * dN * sin(dLambda)) / dP);
00385                 dN = dGps_SemiMajorAxis / (sqrt(1.0 - (dGps_Eccentricity2 * sin(dLambda) * sin(dLambda))));
00386         }
00387 
00388         dLong = atan2(dY_ECEF, dX_ECEF);
00389         dLat = dLambda;
00390 
00391         resetToDefault();
00392         setLongitudeInRad(dLong);       // *pdLong = dLong * (180.0 / GPS_PI);
00393         setLatitudeInRad(dLat);         // *pdLat = dLat * (180.0 / GPS_PI);
00394 }
00395 
00400 Point3D PositionWGS84::getCartesianRelPos(const PositionWGS84& orign) const
00401 {
00402         double x, y;
00403         transformToTangentialPlane(orign, &x, &y);
00404         Point3D pos(x, y, getAltitudeInMeterMSL() - orign.getAltitudeInMeterMSL());
00405         return pos;
00406 }
00407 
00408 std::string toString(const PositionWGS84::PositionWGS84SourceType& type)
00409 {
00410         switch (type)
00411         {
00412         case PositionWGS84::GPS_SPS:
00413                 return "GPS (SPS)";
00414         case PositionWGS84::GPS_PPS:
00415                 return "GPS (PPS)";
00416         case PositionWGS84::GPS_SBAS:
00417                 return "GPS (SBAS)";
00418         case PositionWGS84::GPS_SBAS_Omnistar_VBS:
00419                 return "GPS (Omnistar VBS)";
00420         case PositionWGS84::GPS_SBAS_Omnistar_HP:
00421                 return "GPS (Omnistar HP)";
00422         case PositionWGS84::GPS_GBAS:
00423                 return "GPS (GBAS)";
00424         case PositionWGS84::GPS_GBAS_RTK_Float:
00425                 return "GPS (RTK float)";
00426         case PositionWGS84::GPS_GBAS_RTK_Integer:
00427                 return "GPS (RTK integer)";
00428         case PositionWGS84::IMU:
00429                 return "IMU";
00430         case PositionWGS84::LandmarkPositioning:
00431                 return "Landmark positioning";
00432         case PositionWGS84::Manual:
00433                 return "Manual";
00434         case PositionWGS84::CAN:
00435                 return "CAN";
00436         default:
00437         {
00438                 std::stringstream ret;
00439                 ret << "Unknown (" << type << ")";
00440                 return ret.str();
00441         }
00442         }
00443 }
00444 
00445 }       // namespace datatypes