Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040 #ifndef ROBOT_LOCALIZATION_NAVSAT_CONVERSIONS_H
00041 #define ROBOT_LOCALIZATION_NAVSAT_CONVERSIONS_H
00042
00051 #include <cmath>
00052 #include <string>
00053
00054 #include <stdio.h>
00055 #include <stdlib.h>
00056
00057 namespace RobotLocalization
00058 {
00059 namespace NavsatConversions
00060 {
00061
00062 const double RADIANS_PER_DEGREE = M_PI/180.0;
00063 const double DEGREES_PER_RADIAN = 180.0/M_PI;
00064
00065
00066 const double grid_size = 100000.0;
00067
00068
00069 #define WGS84_A 6378137.0 // major axis
00070 #define WGS84_B 6356752.31424518 // minor axis
00071 #define WGS84_F 0.0033528107 // ellipsoid flattening
00072 #define WGS84_E 0.0818191908 // first eccentricity
00073 #define WGS84_EP 0.0820944379 // second eccentricity
00074
00075
00076 #define UTM_K0 0.9996 // scale factor
00077 #define UTM_FE 500000.0 // false easting
00078 #define UTM_FN_N 0.0 // false northing, northern hemisphere
00079 #define UTM_FN_S 10000000.0 // false northing, southern hemisphere
00080 #define UTM_E2 (WGS84_E*WGS84_E) // e^2
00081 #define UTM_E4 (UTM_E2*UTM_E2) // e^4
00082 #define UTM_E6 (UTM_E4*UTM_E2) // e^6
00083 #define UTM_EP2 (UTM_E2/(1-UTM_E2)) // e'^2
00084
00094 static inline void UTM(double lat, double lon, double *x, double *y)
00095 {
00096
00097 static const double m0 = (1 - UTM_E2/4 - 3*UTM_E4/64 - 5*UTM_E6/256);
00098 static const double m1 = -(3*UTM_E2/8 + 3*UTM_E4/32 + 45*UTM_E6/1024);
00099 static const double m2 = (15*UTM_E4/256 + 45*UTM_E6/1024);
00100 static const double m3 = -(35*UTM_E6/3072);
00101
00102
00103 int cm = ((lon >= 0.0)
00104 ? (static_cast<int>(lon) - (static_cast<int>(lon)) % 6 + 3)
00105 : (static_cast<int>(lon) - (static_cast<int>(lon)) % 6 - 3));
00106
00107
00108 double rlat = lat * RADIANS_PER_DEGREE;
00109 double rlon = lon * RADIANS_PER_DEGREE;
00110 double rlon0 = cm * RADIANS_PER_DEGREE;
00111
00112
00113 double slat = sin(rlat);
00114 double clat = cos(rlat);
00115 double tlat = tan(rlat);
00116
00117
00118 double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;
00119
00120 double T = tlat * tlat;
00121 double C = UTM_EP2 * clat * clat;
00122 double A = (rlon - rlon0) * clat;
00123 double M = WGS84_A * (m0*rlat + m1*sin(2*rlat)
00124 + m2*sin(4*rlat) + m3*sin(6*rlat));
00125 double V = WGS84_A / sqrt(1 - UTM_E2*slat*slat);
00126
00127
00128 *x = UTM_FE + UTM_K0 * V * (A + (1-T+C)*pow(A, 3)/6
00129 + (5-18*T+T*T+72*C-58*UTM_EP2)*pow(A, 5)/120);
00130 *y = fn + UTM_K0 * (M + V * tlat * (A*A/2
00131 + (5-T+9*C+4*C*C)*pow(A, 4)/24
00132 + ((61-58*T+T*T+600*C-330*UTM_EP2)
00133 * pow(A, 6)/720)));
00134
00135 return;
00136 }
00137
00138
00147 static inline char UTMLetterDesignator(double Lat)
00148 {
00149 char LetterDesignator;
00150
00151 if ((84 >= Lat) && (Lat >= 72)) LetterDesignator = 'X';
00152 else if ((72 > Lat) && (Lat >= 64)) LetterDesignator = 'W';
00153 else if ((64 > Lat) && (Lat >= 56)) LetterDesignator = 'V';
00154 else if ((56 > Lat) && (Lat >= 48)) LetterDesignator = 'U';
00155 else if ((48 > Lat) && (Lat >= 40)) LetterDesignator = 'T';
00156 else if ((40 > Lat) && (Lat >= 32)) LetterDesignator = 'S';
00157 else if ((32 > Lat) && (Lat >= 24)) LetterDesignator = 'R';
00158 else if ((24 > Lat) && (Lat >= 16)) LetterDesignator = 'Q';
00159 else if ((16 > Lat) && (Lat >= 8)) LetterDesignator = 'P';
00160 else if (( 8 > Lat) && (Lat >= 0)) LetterDesignator = 'N';
00161 else if (( 0 > Lat) && (Lat >= -8)) LetterDesignator = 'M';
00162 else if ((-8 > Lat) && (Lat >= -16)) LetterDesignator = 'L';
00163 else if ((-16 > Lat) && (Lat >= -24)) LetterDesignator = 'K';
00164 else if ((-24 > Lat) && (Lat >= -32)) LetterDesignator = 'J';
00165 else if ((-32 > Lat) && (Lat >= -40)) LetterDesignator = 'H';
00166 else if ((-40 > Lat) && (Lat >= -48)) LetterDesignator = 'G';
00167 else if ((-48 > Lat) && (Lat >= -56)) LetterDesignator = 'F';
00168 else if ((-56 > Lat) && (Lat >= -64)) LetterDesignator = 'E';
00169 else if ((-64 > Lat) && (Lat >= -72)) LetterDesignator = 'D';
00170 else if ((-72 > Lat) && (Lat >= -80)) LetterDesignator = 'C';
00171
00172 else LetterDesignator = 'Z';
00173 return LetterDesignator;
00174 }
00175
00185 static inline void LLtoUTM(const double Lat, const double Long,
00186 double &UTMNorthing, double &UTMEasting,
00187 std::string &UTMZone)
00188 {
00189 double a = WGS84_A;
00190 double eccSquared = UTM_E2;
00191 double k0 = UTM_K0;
00192
00193 double LongOrigin;
00194 double eccPrimeSquared;
00195 double N, T, C, A, M;
00196
00197
00198 double LongTemp = (Long+180)-static_cast<int>((Long+180)/360)*360-180;
00199
00200 double LatRad = Lat*RADIANS_PER_DEGREE;
00201 double LongRad = LongTemp*RADIANS_PER_DEGREE;
00202 double LongOriginRad;
00203 int ZoneNumber;
00204
00205 ZoneNumber = static_cast<int>((LongTemp + 180)/6) + 1;
00206
00207 if ( Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 )
00208 ZoneNumber = 32;
00209
00210
00211 if ( Lat >= 72.0 && Lat < 84.0 )
00212 {
00213 if ( LongTemp >= 0.0 && LongTemp < 9.0 ) ZoneNumber = 31;
00214 else if ( LongTemp >= 9.0 && LongTemp < 21.0 ) ZoneNumber = 33;
00215 else if ( LongTemp >= 21.0 && LongTemp < 33.0 ) ZoneNumber = 35;
00216 else if ( LongTemp >= 33.0 && LongTemp < 42.0 ) ZoneNumber = 37;
00217 }
00218
00219 LongOrigin = (ZoneNumber - 1)*6 - 180 + 3;
00220 LongOriginRad = LongOrigin * RADIANS_PER_DEGREE;
00221
00222
00223 char zone_buf[] = {0, 0, 0, 0};
00224 snprintf(zone_buf, sizeof(zone_buf), "%d%c", ZoneNumber, UTMLetterDesignator(Lat));
00225 UTMZone = std::string(zone_buf);
00226
00227 eccPrimeSquared = (eccSquared)/(1-eccSquared);
00228
00229 N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad));
00230 T = tan(LatRad)*tan(LatRad);
00231 C = eccPrimeSquared*cos(LatRad)*cos(LatRad);
00232 A = cos(LatRad)*(LongRad-LongOriginRad);
00233
00234 M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64
00235 - 5*eccSquared*eccSquared*eccSquared/256) * LatRad
00236 - (3*eccSquared/8 + 3*eccSquared*eccSquared/32
00237 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
00238 + (15*eccSquared*eccSquared/256
00239 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad)
00240 - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad));
00241
00242 UTMEasting = static_cast<double>
00243 (k0*N*(A+(1-T+C)*A*A*A/6
00244 + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
00245 + 500000.0);
00246
00247 UTMNorthing = static_cast<double>
00248 (k0*(M+N*tan(LatRad)
00249 *(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
00250 + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720)));
00251
00252 if (Lat < 0)
00253 {
00254
00255 UTMNorthing += 10000000.0;
00256 }
00257 }
00258
00268 static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting,
00269 const std::string &UTMZone, double& Lat, double& Long )
00270 {
00271 double k0 = UTM_K0;
00272 double a = WGS84_A;
00273 double eccSquared = UTM_E2;
00274 double eccPrimeSquared;
00275 double e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared));
00276 double N1, T1, C1, R1, D, M;
00277 double LongOrigin;
00278 double mu, phi1Rad;
00279 double x, y;
00280 int ZoneNumber;
00281 char* ZoneLetter;
00282
00283 x = UTMEasting - 500000.0;
00284 y = UTMNorthing;
00285
00286 ZoneNumber = strtoul(UTMZone.c_str(), &ZoneLetter, 10);
00287 if ((*ZoneLetter - 'N') < 0)
00288 {
00289
00290 y -= 10000000.0;
00291 }
00292
00293
00294 LongOrigin = (ZoneNumber - 1)*6 - 180 + 3;
00295 eccPrimeSquared = (eccSquared)/(1-eccSquared);
00296
00297 M = y / k0;
00298 mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64
00299 -5*eccSquared*eccSquared*eccSquared/256));
00300
00301 phi1Rad = mu + ((3*e1/2-27*e1*e1*e1/32)*sin(2*mu)
00302 + (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu)
00303 + (151*e1*e1*e1/96)*sin(6*mu));
00304
00305 N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad));
00306 T1 = tan(phi1Rad)*tan(phi1Rad);
00307 C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad);
00308 R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5);
00309 D = x/(N1*k0);
00310
00311 Lat = phi1Rad - ((N1*tan(phi1Rad)/R1)
00312 *(D*D/2
00313 -(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
00314 +(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared
00315 -3*C1*C1)*D*D*D*D*D*D/720));
00316
00317 Lat = Lat * DEGREES_PER_RADIAN;
00318
00319 Long = ((D-(1+2*T1+C1)*D*D*D/6
00320 +(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)
00321 *D*D*D*D*D/120)
00322 / cos(phi1Rad));
00323 Long = LongOrigin + Long * DEGREES_PER_RADIAN;
00324 }
00325
00326 }
00327 }
00328
00329 #endif // ROBOT_LOCALIZATION_NAVSAT_CONVERSIONS_H