geodesy: geodesy: utm_conversions.cpp Source File

00001 /* $Id: 8b94f2e1fa9389bb9b9229465157c45e84029c98 $ */
00002 
00003 /*********************************************************************
00004 * Software License Agreement (BSD License)
00005 *
00006 *  Copyright (C) 2011 Chuck Gantz, Jack O'Quin
00007 *  All rights reserved.
00008 *
00009 *  Redistribution and use in source and binary forms, with or without
00010 *  modification, are permitted provided that the following conditions
00011 *  are met:
00012 *
00013 *   * Redistributions of source code must retain the above copyright
00014 *     notice, this list of conditions and the following disclaimer.
00015 *   * Redistributions in binary form must reproduce the above
00016 *     copyright notice, this list of conditions and the following
00017 *     disclaimer in the documentation and/or other materials provided
00018 *     with the distribution.
00019 *   * Neither the name of the author nor other contributors may be
00020 *     used to endorse or promote products derived from this software
00021 *     without specific prior written permission.
00022 *
00023 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 *  POSSIBILITY OF SUCH DAMAGE.
00035 *********************************************************************/
00036 
00037 //#include <exception>
00038 #include <ros/ros.h>
00039 #include <angles/angles.h>
00040 #include <geodesy/utm.h>
00041 
00055 namespace geodesy
00056 {
00057 
00058 // WGS84 Parameters
00059 #define WGS84_A         6378137.0               // major axis
00060 #define WGS84_B         6356752.31424518        // minor axis
00061 #define WGS84_F         0.0033528107            // ellipsoid flattening
00062 #define WGS84_E         0.0818191908            // first eccentricity
00063 #define WGS84_EP        0.0820944379            // second eccentricity
00064 
00065 // UTM Parameters
00066 #define UTM_K0          0.9996                  // scale factor
00067 #define UTM_FE          500000.0                // false easting
00068 #define UTM_FN_N        0.0           // false northing, northern hemisphere
00069 #define UTM_FN_S        10000000.0    // false northing, southern hemisphere
00070 #define UTM_E2          (WGS84_E*WGS84_E)       // e^2
00071 #define UTM_E4          (UTM_E2*UTM_E2)         // e^4
00072 #define UTM_E6          (UTM_E4*UTM_E2)         // e^6
00073 #define UTM_EP2         (UTM_E2/(1-UTM_E2))     // e'^2
00074 
00075 
00082 static char UTMBand(double Lat, double Lon)
00083 {
00084   char LetterDesignator;
00085 
00086   if     ((84 >= Lat) && (Lat >= 72))  LetterDesignator = 'X';
00087   else if ((72 > Lat) && (Lat >= 64))  LetterDesignator = 'W';
00088   else if ((64 > Lat) && (Lat >= 56))  LetterDesignator = 'V';
00089   else if ((56 > Lat) && (Lat >= 48))  LetterDesignator = 'U';
00090   else if ((48 > Lat) && (Lat >= 40))  LetterDesignator = 'T';
00091   else if ((40 > Lat) && (Lat >= 32))  LetterDesignator = 'S';
00092   else if ((32 > Lat) && (Lat >= 24))  LetterDesignator = 'R';
00093   else if ((24 > Lat) && (Lat >= 16))  LetterDesignator = 'Q';
00094   else if ((16 > Lat) && (Lat >= 8))   LetterDesignator = 'P';
00095   else if (( 8 > Lat) && (Lat >= 0))   LetterDesignator = 'N';
00096   else if (( 0 > Lat) && (Lat >= -8))  LetterDesignator = 'M';
00097   else if ((-8 > Lat) && (Lat >= -16)) LetterDesignator = 'L';
00098   else if((-16 > Lat) && (Lat >= -24)) LetterDesignator = 'K';
00099   else if((-24 > Lat) && (Lat >= -32)) LetterDesignator = 'J';
00100   else if((-32 > Lat) && (Lat >= -40)) LetterDesignator = 'H';
00101   else if((-40 > Lat) && (Lat >= -48)) LetterDesignator = 'G';
00102   else if((-48 > Lat) && (Lat >= -56)) LetterDesignator = 'F';
00103   else if((-56 > Lat) && (Lat >= -64)) LetterDesignator = 'E';
00104   else if((-64 > Lat) && (Lat >= -72)) LetterDesignator = 'D';
00105   else if((-72 > Lat) && (Lat >= -80)) LetterDesignator = 'C';
00106   // '_' is an error flag, the Latitude is outside the UTM limits
00107   else LetterDesignator = ' ';
00108 
00109   return LetterDesignator;
00110 }
00111 
00119 geographic_msgs::GeoPoint toMsg(const UTMPoint &from)
00120 {
00121   //remove 500,000 meter offset for longitude
00122   double x = from.easting - 500000.0;
00123   double y = from.northing;
00124 
00125   double k0 = UTM_K0;
00126   double a = WGS84_A;
00127   double eccSquared = UTM_E2;
00128   double eccPrimeSquared;
00129   double e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared));
00130   double N1, T1, C1, R1, D, M;
00131   double LongOrigin;
00132   double mu, phi1Rad;
00133 
00134   if ((from.band - 'N') < 0)
00135     {
00136       //point is in southern hemisphere
00137       //remove 10,000,000 meter offset used for southern hemisphere
00138       y -= 10000000.0;
00139     }
00140 
00141   //+3 puts origin in middle of zone
00142   LongOrigin = (from.zone - 1)*6 - 180 + 3;
00143   eccPrimeSquared = (eccSquared)/(1-eccSquared);
00144 
00145   M = y / k0;
00146   mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64
00147              -5*eccSquared*eccSquared*eccSquared/256));
00148 
00149   phi1Rad = mu + ((3*e1/2-27*e1*e1*e1/32)*sin(2*mu) 
00150                   + (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu)
00151                   + (151*e1*e1*e1/96)*sin(6*mu));
00152 
00153   N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad));
00154   T1 = tan(phi1Rad)*tan(phi1Rad);
00155   C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad);
00156   R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5);
00157   D = x/(N1*k0);
00158 
00159   // function result
00160   geographic_msgs::GeoPoint to;
00161   to.altitude = from.altitude;
00162   to.latitude =
00163     phi1Rad - ((N1*tan(phi1Rad)/R1)
00164                *(D*D/2
00165                  -(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
00166                  +(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared
00167                    -3*C1*C1)*D*D*D*D*D*D/720));
00168   to.latitude = angles::to_degrees(to.latitude);
00169   to.longitude = ((D-(1+2*T1+C1)*D*D*D/6
00170                    +(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)
00171                    *D*D*D*D*D/120)
00172                   / cos(phi1Rad));
00173   to.longitude = LongOrigin + angles::to_degrees(to.longitude);
00174 
00175   // Normalize latitude and longitude to valid ranges.
00176   normalize(to);
00177   return to;
00178 }
00179 
00187 void fromMsg(const geographic_msgs::GeoPoint &from, UTMPoint &to)
00188 {
00189   double Lat = from.latitude;
00190   double Long = from.longitude;
00191 
00192   double a = WGS84_A;
00193   double eccSquared = UTM_E2;
00194   double k0 = UTM_K0;
00195 
00196   double LongOrigin;
00197   double eccPrimeSquared;
00198   double N, T, C, A, M;
00199         
00200   // Make sure the longitude is between -180.00 .. 179.9
00201   // (JOQ: this is broken for Long < -180, do a real normalize)
00202   double LongTemp = (Long+180)-int((Long+180)/360)*360-180;
00203   double LatRad = angles::from_degrees(Lat);
00204   double LongRad = angles::from_degrees(LongTemp);
00205   double LongOriginRad;
00206 
00207   to.altitude = from.altitude;
00208   to.zone = int((LongTemp + 180)/6) + 1;
00209   
00210   if( Lat >= 56.0 && Lat < 64.0 && LongTemp >= 3.0 && LongTemp < 12.0 )
00211     to.zone = 32;
00212 
00213   // Special zones for Svalbard
00214   if( Lat >= 72.0 && Lat < 84.0 ) 
00215     {
00216       if(      LongTemp >= 0.0  && LongTemp <  9.0 ) to.zone = 31;
00217       else if( LongTemp >= 9.0  && LongTemp < 21.0 ) to.zone = 33;
00218       else if( LongTemp >= 21.0 && LongTemp < 33.0 ) to.zone = 35;
00219       else if( LongTemp >= 33.0 && LongTemp < 42.0 ) to.zone = 37;
00220     }
00221   // +3 puts origin in middle of zone
00222   LongOrigin = (to.zone - 1)*6 - 180 + 3; 
00223   LongOriginRad = angles::from_degrees(LongOrigin);
00224 
00225   // compute the UTM band from the latitude
00226   to.band = UTMBand(Lat, LongTemp);
00227 #if 0
00228   if (to.band == ' ')
00229     throw std::range_error;
00230 #endif
00231 
00232   eccPrimeSquared = (eccSquared)/(1-eccSquared);
00233 
00234   N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad));
00235   T = tan(LatRad)*tan(LatRad);
00236   C = eccPrimeSquared*cos(LatRad)*cos(LatRad);
00237   A = cos(LatRad)*(LongRad-LongOriginRad);
00238 
00239   M = a*((1 - eccSquared/4 - 3*eccSquared*eccSquared/64
00240           - 5*eccSquared*eccSquared*eccSquared/256) * LatRad 
00241          - (3*eccSquared/8 + 3*eccSquared*eccSquared/32
00242             + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
00243          + (15*eccSquared*eccSquared/256
00244             + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) 
00245          - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad));
00246         
00247   to.easting = (double)
00248     (k0*N*(A+(1-T+C)*A*A*A/6
00249            + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
00250      + 500000.0);
00251 
00252   to.northing = (double)
00253     (k0*(M+N*tan(LatRad)
00254          *(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
00255            + (61-58*T+T*T+600*C-330*eccPrimeSquared)*A*A*A*A*A*A/720)));
00256 
00257   if(Lat < 0)
00258     {
00259       //10000000 meter offset for southern hemisphere
00260       to.northing += 10000000.0;
00261     }
00262 }
00263 
00265 bool isValid(const UTMPoint &pt)
00266 {
00267   if (pt.zone < 1 || pt.zone > 60)
00268     return false;
00269 
00270   if (!isupper(pt.band) || pt.band == 'I' || pt.band == 'O')
00271     return false;
00272 
00273   // The Universal Polar Stereographic bands are not currently
00274   // supported.  When they are: A, B, Y and Z will be valid (and the
00275   // zone number will be ignored).
00276   if (pt.band < 'C' || pt.band > 'X')
00277     return false;
00278 
00279   return true;
00280 }
00281  
00283 UTMPoint::UTMPoint(const geographic_msgs::GeoPoint &pt)
00284 {
00285   fromMsg(pt, *this);
00286 }
00287 
00295 void fromMsg(const geographic_msgs::GeoPose &from, UTMPose &to)
00296 {
00297   fromMsg(from.position, to.position);
00298   to.orientation = from.orientation;
00299 }
00300 
00302 bool isValid(const UTMPose &pose)
00303 {
00304   if (!isValid(pose.position))
00305     return false;
00306 
00307   // check that orientation quaternion is normalized
00308   double len2 = (pose.orientation.x * pose.orientation.x
00309                  + pose.orientation.y * pose.orientation.y
00310                  + pose.orientation.z * pose.orientation.z
00311                  + pose.orientation.w * pose.orientation.w);
00312   return fabs(len2 - 1.0) <= tf::QUATERNION_TOLERANCE;
00313 }
00314 
00315 } // end namespace geodesy