robot_localization: navsat_conversions.h Source File

Go to the documentation of this file.
00001 /*
00002 * Copyright (C) 2010 Austin Robot Technology, and others
00003 * All rights reserved.
00004 *
00005 * Redistribution and use in source and binary forms, with or without
00006 * modification, are permitted provided that the following conditions
00007 * are met:
00008 *
00009 * 1. Redistributions of source code must retain the above copyright
00010 * notice, this list of conditions and the following disclaimer.
00011 * 2. Redistributions in binary form must reproduce the above
00012 * copyright notice, this list of conditions and the following
00013 * disclaimer in the documentation and/or other materials provided
00014 * with the distribution.
00015 * 3. Neither the names of the University of Texas at Austin, nor
00016 * Austin Robot Technology, nor the names of other contributors may
00017 * be used to endorse or promote products derived from this
00018 * software without specific prior written permission.
00019 *
00020 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 * POSSIBILITY OF SUCH DAMAGE.
00032 *
00033 * This file contains code from multiple files in the original
00034 * source. The originals can be found here:
00035 *
00036 * https://github.com/austin-robot/utexas-art-ros-pkg/blob/afd147a1eb944fc3dbd138574c39699813f797bf/stacks/art_vehicle/art_common/include/art/UTM.h
00037 * https://github.com/austin-robot/utexas-art-ros-pkg/blob/afd147a1eb944fc3dbd138574c39699813f797bf/stacks/art_vehicle/art_common/include/art/conversions.h
00038 */
00039 
00040 #ifndef RobotLocalization_NavsatConversions_h
00041 #define RobotLocalization_NavsatConversions_h
00042 
00051 #include <cmath>
00052 #include <stdio.h>
00053 #include <stdlib.h>
00054 
00055 namespace RobotLocalization
00056 {
00057   namespace NavsatConversions
00058   {
00059     const double RADIANS_PER_DEGREE = M_PI/180.0;
00060     const double DEGREES_PER_RADIAN = 180.0/M_PI;
00061 
00062     // Grid granularity for rounding UTM coordinates to generate MapXY.
00063     const double grid_size = 100000.0;    // 100 km grid
00064 
00065     // WGS84 Parameters
00066     #define WGS84_A   6378137.0   // major axis
00067     #define WGS84_B   6356752.31424518  // minor axis
00068     #define WGS84_F   0.0033528107    // ellipsoid flattening
00069     #define WGS84_E   0.0818191908    // first eccentricity
00070     #define WGS84_EP  0.0820944379    // second eccentricity
00071 
00072     // UTM Parameters
00073     #define UTM_K0    0.9996      // scale factor
00074     #define UTM_FE    500000.0    // false easting
00075     #define UTM_FN_N  0.0           // false northing, northern hemisphere
00076     #define UTM_FN_S  10000000.0    // false northing, southern hemisphere
00077     #define UTM_E2    (WGS84_E*WGS84_E) // e^2
00078     #define UTM_E4    (UTM_E2*UTM_E2)   // e^4
00079     #define UTM_E6    (UTM_E4*UTM_E2)   // e^6
00080     #define UTM_EP2   (UTM_E2/(1-UTM_E2)) // e'^2
00081 
00091     static inline void UTM(double lat, double lon, double *x, double *y)
00092     {
00093       // constants
00094       const static double m0 = (1 - UTM_E2/4 - 3*UTM_E4/64 - 5*UTM_E6/256);
00095       const static double m1 = -(3*UTM_E2/8 + 3*UTM_E4/32 + 45*UTM_E6/1024);
00096       const static double m2 = (15*UTM_E4/256 + 45*UTM_E6/1024);
00097       const static double m3 = -(35*UTM_E6/3072);
00098 
00099       // compute the central meridian
00100       int cm = ((lon >= 0.0)
00101           ? ((int)lon - ((int)lon)%6 + 3)
00102           : ((int)lon - ((int)lon)%6 - 3));
00103 
00104       // convert degrees into radians
00105       double rlat = lat * RADIANS_PER_DEGREE;
00106       double rlon = lon * RADIANS_PER_DEGREE;
00107       double rlon0 = cm * RADIANS_PER_DEGREE;
00108 
00109       // compute trigonometric functions
00110       double slat = sin(rlat);
00111       double clat = cos(rlat);
00112       double tlat = tan(rlat);
00113 
00114       // decide the false northing at origin
00115       double fn = (lat > 0) ? UTM_FN_N : UTM_FN_S;
00116 
00117       double T = tlat * tlat;
00118       double C = UTM_EP2 * clat * clat;
00119       double A = (rlon - rlon0) * clat;
00120       double M = WGS84_A * (m0*rlat + m1*sin(2*rlat)
00121           + m2*sin(4*rlat) + m3*sin(6*rlat));
00122       double V = WGS84_A / sqrt(1 - UTM_E2*slat*slat);
00123 
00124       // compute the easting-northing coordinates
00125       *x = UTM_FE + UTM_K0 * V * (A + (1-T+C)*pow(A,3)/6
00126                 + (5-18*T+T*T+72*C-58*UTM_EP2)*pow(A,5)/120);
00127       *y = fn + UTM_K0 * (M + V * tlat * (A*A/2
00128                   + (5-T+9*C+4*C*C)*pow(A,4)/24
00129                   + ((61-58*T+T*T+600*C-330*UTM_EP2)
00130                * pow(A,6)/720)));
00131 
00132       return;
00133     }
00134 
00135 
00144     static inline char UTMLetterDesignator(double Lat)
00145     {
00146       char LetterDesignator;
00147 
00148       if     ((84 >= Lat) && (Lat >= 72))  LetterDesignator = 'X';
00149       else if ((72 > Lat) && (Lat >= 64))  LetterDesignator = 'W';
00150       else if ((64 > Lat) && (Lat >= 56))  LetterDesignator = 'V';
00151       else if ((56 > Lat) && (Lat >= 48))  LetterDesignator = 'U';
00152       else if ((48 > Lat) && (Lat >= 40))  LetterDesignator = 'T';
00153       else if ((40 > Lat) && (Lat >= 32))  LetterDesignator = 'S';
00154       else if ((32 > Lat) && (Lat >= 24))  LetterDesignator = 'R';
00155       else if ((24 > Lat) && (Lat >= 16))  LetterDesignator = 'Q';
00156       else if ((16 > Lat) && (Lat >= 8))   LetterDesignator = 'P';
00157       else if (( 8 > Lat) && (Lat >= 0))   LetterDesignator = 'N';
00158       else if (( 0 > Lat) && (Lat >= -8))  LetterDesignator = 'M';
00159       else if ((-8 > Lat) && (Lat >= -16)) LetterDesignator = 'L';
00160       else if((-16 > Lat) && (Lat >= -24)) LetterDesignator = 'K';
00161       else if((-24 > Lat) && (Lat >= -32)) LetterDesignator = 'J';
00162       else if((-32 > Lat) && (Lat >= -40)) LetterDesignator = 'H';
00163       else if((-40 > Lat) && (Lat >= -48)) LetterDesignator = 'G';
00164       else if((-48 > Lat) && (Lat >= -56)) LetterDesignator = 'F';
00165       else if((-56 > Lat) && (Lat >= -64)) LetterDesignator = 'E';
00166       else if((-64 > Lat) && (Lat >= -72)) LetterDesignator = 'D';
00167       else if((-72 > Lat) && (Lat >= -80)) LetterDesignator = 'C';
00168             // 'Z' is an error flag, the Latitude is outside the UTM limits
00169       else LetterDesignator = 'Z';
00170       return LetterDesignator;
00171     }
00172 
00182     static inline void LLtoUTM(const double Lat, const double Long,
00183                                double &UTMNorthing, double &UTMEasting,
00184                                std::string &UTMZone)
00185     {
00186       double a = WGS84_A;
00187       double eccSquared = UTM_E2;
00188       double k0 = UTM_K0;
00189 
00190       double LongOrigin;
00191       double eccPrimeSquared;
00192       double N, T, C, A, M;
00193 
00194             //Make sure the longitude is between -180.00 .. 179.9
00195       double LongTemp = (Long+180)-int((Long+180)/360)*360-180;
00196 
00197       double LatRad = Lat*RADIANS_PER_DEGREE;
00198       double LongRad = LongTemp*RADIANS_PER_DEGREE;
00199       double LongOriginRad;
00200       int    ZoneNumber;
00201 
00202       ZoneNumber = int((LongTemp + 180)/6) + 1;
00203 
00204       if( Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 )
00205         ZoneNumber = 32;
00206 
00207             // Special zones for Svalbard
00208       if( Lat >= 72.0 && Lat < 84.0 )
00209       {
00210         if(      LongTemp >= 0.0  && LongTemp <  9.0 ) ZoneNumber = 31;
00211         else if( LongTemp >= 9.0  && LongTemp < 21.0 ) ZoneNumber = 33;
00212         else if( LongTemp >= 21.0 && LongTemp < 33.0 ) ZoneNumber = 35;
00213         else if( LongTemp >= 33.0 && LongTemp < 42.0 ) ZoneNumber = 37;
00214        }
00215             // +3 puts origin in middle of zone
00216       LongOrigin = (ZoneNumber - 1)*6 - 180 + 3;
00217       LongOriginRad = LongOrigin * RADIANS_PER_DEGREE;
00218 
00219       //compute the UTM Zone from the latitude and longitude
00220       char zone_buf[] = {0, 0, 0, 0};
00221       sprintf(zone_buf, "%d%c", ZoneNumber, UTMLetterDesignator(Lat));
00222       UTMZone = std::string(zone_buf);
00223 
00224       eccPrimeSquared = (eccSquared)/(1-eccSquared);
00225 
00226       N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad));
00227       T = tan(LatRad)*tan(LatRad);
00228       C = eccPrimeSquared*cos(LatRad)*cos(LatRad);
00229       A = cos(LatRad)*(LongRad-LongOriginRad);
00230 
00231       M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64
00232                     - 5*eccSquared*eccSquared*eccSquared/256) * LatRad
00233                    - (3*eccSquared/8 + 3*eccSquared*eccSquared/32
00234                       + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
00235                    + (15*eccSquared*eccSquared/256
00236                       + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad)
00237                    - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad));
00238 
00239       UTMEasting = (double)
00240               (k0*N*(A+(1-T+C)*A*A*A/6
00241                      + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
00242                + 500000.0);
00243 
00244       UTMNorthing = (double)
00245               (k0*(M+N*tan(LatRad)
00246                    *(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
00247                      + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720)));
00248 
00249       if(Lat < 0)
00250               {
00251                 //10000000 meter offset for southern hemisphere
00252                 UTMNorthing += 10000000.0;
00253               }
00254     }
00255 
00265     static inline void UTMtoLL(const double UTMNorthing, const double UTMEasting,
00266                                const std::string &UTMZone, double& Lat,  double& Long )
00267     {
00268       double k0 = UTM_K0;
00269       double a = WGS84_A;
00270       double eccSquared = UTM_E2;
00271       double eccPrimeSquared;
00272       double e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared));
00273       double N1, T1, C1, R1, D, M;
00274       double LongOrigin;
00275       double mu, phi1Rad;
00276       double x, y;
00277       int ZoneNumber;
00278       char* ZoneLetter;
00279 
00280       x = UTMEasting - 500000.0; //remove 500,000 meter offset for longitude
00281       y = UTMNorthing;
00282 
00283       ZoneNumber = strtoul(UTMZone.c_str(), &ZoneLetter, 10);
00284       if((*ZoneLetter - 'N') < 0)
00285               {
00286                 //remove 10,000,000 meter offset used for southern hemisphere
00287                 y -= 10000000.0;
00288               }
00289 
00290             //+3 puts origin in middle of zone
00291       LongOrigin = (ZoneNumber - 1)*6 - 180 + 3;
00292       eccPrimeSquared = (eccSquared)/(1-eccSquared);
00293 
00294       M = y / k0;
00295       mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64
00296                        -5*eccSquared*eccSquared*eccSquared/256));
00297 
00298       phi1Rad = mu + ((3*e1/2-27*e1*e1*e1/32)*sin(2*mu)
00299                             + (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu)
00300                             + (151*e1*e1*e1/96)*sin(6*mu));
00301 
00302       N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad));
00303       T1 = tan(phi1Rad)*tan(phi1Rad);
00304       C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad);
00305       R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5);
00306       D = x/(N1*k0);
00307 
00308       Lat = phi1Rad - ((N1*tan(phi1Rad)/R1)
00309                              *(D*D/2
00310                                -(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
00311                                +(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared
00312                                  -3*C1*C1)*D*D*D*D*D*D/720));
00313 
00314       Lat = Lat * DEGREES_PER_RADIAN;
00315 
00316       Long = ((D-(1+2*T1+C1)*D*D*D/6
00317                      +(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)
00318                      *D*D*D*D*D/120)
00319                     / cos(phi1Rad));
00320       Long = LongOrigin + Long * DEGREES_PER_RADIAN;
00321 
00322     }
00323   } // end namespace UTM
00324 }
00325 
00326 #endif // _UTM_H