applanix_publisher: applanix_publisher: gps

00001 #! /usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 #     _____
00004 #    /  _  \
00005 #   / _/ \  \
00006 #  / / \_/   \
00007 # /  \_/  _   \  ___  _    ___   ___   ____   ____   ___   _____  _   _
00008 # \  / \_/ \  / /  _\| |  | __| / _ \ | ┌┐ \ | ┌┐ \ / _ \ |_   _|| | | |
00009 #  \ \_/ \_/ /  | |  | |  | └─┐| |_| || └┘ / | └┘_/| |_| |  | |  | └─┘ |
00010 #   \  \_/  /   | |_ | |_ | ┌─┘|  _  || |\ \ | |   |  _  |  | |  | ┌─┐ |
00011 #    \_____/    \___/|___||___||_| |_||_| \_\|_|   |_| |_|  |_|  |_| |_|
00012 #            ROBOTICS™
00013 #
00014 #
00015 #  Copyright © 2012 Clearpath Robotics, Inc. 
00016 #  All Rights Reserved
00017 #  
00018 # Redistribution and use in source and binary forms, with or without
00019 # modification, are permitted provided that the following conditions are met:
00020 #     * Redistributions of source code must retain the above copyright
00021 #       notice, this list of conditions and the following disclaimer.
00022 #     * Redistributions in binary form must reproduce the above copyright
00023 #       notice, this list of conditions and the following disclaimer in the
00024 #       documentation and/or other materials provided with the distribution.
00025 #     * Neither the name of Clearpath Robotics, Inc. nor the
00026 #       names of its contributors may be used to endorse or promote products
00027 #       derived from this software without specific prior written permission.
00028 # 
00029 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00030 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00031 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00032 # DISCLAIMED. IN NO EVENT SHALL CLEARPATH ROBOTICS, INC. BE LIABLE FOR ANY
00033 # DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00034 # (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00035 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00036 # ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00037 # (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00038 # SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00039 #
00040 # Please send comments, questions, or patches to skynet@clearpathrobotics.com
00041 #
00042 
00043 # Lat Long - UTM, UTM - Lat Long conversions
00044 from math import pi, sin, cos, tan, sqrt
00045 
00046 #LatLong- UTM conversion..h
00047 #definitions for lat/long to UTM and UTM to lat/lng conversions
00048 #include <string.h>
00049 
00050 _deg2rad = pi / 180.0
00051 _rad2deg = 180.0 / pi
00052 
00053 _EquatorialRadius = 2
00054 _eccentricitySquared = 3
00055 
00056 _ellipsoid = [
00057 #  id, Ellipsoid name, Equatorial Radius, square of eccentricity        
00058 # first once is a placeholder only, To allow array indices to match id numbers
00059         [ -1, "Placeholder", 0, 0],
00060         [ 1, "Airy", 6377563, 0.00667054],
00061         [ 2, "Australian National", 6378160, 0.006694542],
00062         [ 3, "Bessel 1841", 6377397, 0.006674372],
00063         [ 4, "Bessel 1841 (Nambia] ", 6377484, 0.006674372],
00064         [ 5, "Clarke 1866", 6378206, 0.006768658],
00065         [ 6, "Clarke 1880", 6378249, 0.006803511],
00066         [ 7, "Everest", 6377276, 0.006637847],
00067         [ 8, "Fischer 1960 (Mercury] ", 6378166, 0.006693422],
00068         [ 9, "Fischer 1968", 6378150, 0.006693422],
00069         [ 10, "GRS 1967", 6378160, 0.006694605],
00070         [ 11, "GRS 1980", 6378137, 0.00669438],
00071         [ 12, "Helmert 1906", 6378200, 0.006693422],
00072         [ 13, "Hough", 6378270, 0.00672267],
00073         [ 14, "International", 6378388, 0.00672267],
00074         [ 15, "Krassovsky", 6378245, 0.006693422],
00075         [ 16, "Modified Airy", 6377340, 0.00667054],
00076         [ 17, "Modified Everest", 6377304, 0.006637847],
00077         [ 18, "Modified Fischer 1960", 6378155, 0.006693422],
00078         [ 19, "South American 1969", 6378160, 0.006694542],
00079         [ 20, "WGS 60", 6378165, 0.006693422],
00080         [ 21, "WGS 66", 6378145, 0.006694542],
00081         [ 22, "WGS-72", 6378135, 0.006694318],
00082         [ 23, "WGS-84", 6378137, 0.00669438]
00083 ]
00084 
00085 #Reference ellipsoids derived from Peter H. Dana's website- 
00086 #http://www.utexas.edu/depts/grg/gcraft/notes/datum/elist.html
00087 #Department of Geography, University of Texas at Austin
00088 #Internet: pdana@mail.utexas.edu
00089 #3/22/95
00090 
00091 #Source
00092 #Defense Mapping Agency. 1987b. DMA Technical Report: Supplement to Department of Defense World Geodetic System
00093 #1984 Technical Report. Part I and II. Washington, DC: Defense Mapping Agency
00094 
00095 #def LLtoUTM(int ReferenceEllipsoid, const double Lat, const double Long, 
00096 #                        double &UTMNorthing, double &UTMEasting, char* UTMZone)
00097 
00098 def LLtoUTM(ReferenceEllipsoid, Lat, Long, zone = None):
00099     a = _ellipsoid[ReferenceEllipsoid][_EquatorialRadius]
00100     eccSquared = _ellipsoid[ReferenceEllipsoid][_eccentricitySquared]
00101     k0 = 0.9996
00102 
00103 #Make sure the longitude is between -180.00 .. 179.9
00104     LongTemp = (Long+180)-int((Long+180)/360)*360-180 # -180.00 .. 179.9
00105 
00106     LatRad = Lat*_deg2rad
00107     LongRad = LongTemp*_deg2rad
00108 
00109     if zone is None:
00110         ZoneNumber = int((LongTemp + 180)/6) + 1
00111     else:
00112         ZoneNumber = zone
00113   
00114     if Lat >= 56.0 and Lat < 64.0 and LongTemp >= 3.0 and LongTemp < 12.0:
00115         ZoneNumber = 32
00116 
00117     # Special zones for Svalbard
00118     if Lat >= 72.0 and Lat < 84.0:
00119         if  LongTemp >= 0.0  and LongTemp <  9.0:ZoneNumber = 31
00120         elif LongTemp >= 9.0  and LongTemp < 21.0: ZoneNumber = 33
00121         elif LongTemp >= 21.0 and LongTemp < 33.0: ZoneNumber = 35
00122         elif LongTemp >= 33.0 and LongTemp < 42.0: ZoneNumber = 37
00123 
00124     LongOrigin = (ZoneNumber - 1)*6 - 180 + 3 #+3 puts origin in middle of zone
00125     LongOriginRad = LongOrigin * _deg2rad
00126 
00127     #compute the UTM Zone from the latitude and longitude
00128     UTMZone = "%d%c" % (ZoneNumber, _UTMLetterDesignator(Lat))
00129 
00130     eccPrimeSquared = (eccSquared)/(1-eccSquared)
00131     N = a/sqrt(1-eccSquared*sin(LatRad)*sin(LatRad))
00132     T = tan(LatRad)*tan(LatRad)
00133     C = eccPrimeSquared*cos(LatRad)*cos(LatRad)
00134     A = cos(LatRad)*(LongRad-LongOriginRad)
00135 
00136     M = a*((1
00137             - eccSquared/4
00138             - 3*eccSquared*eccSquared/64
00139             - 5*eccSquared*eccSquared*eccSquared/256)*LatRad 
00140            - (3*eccSquared/8
00141               + 3*eccSquared*eccSquared/32
00142               + 45*eccSquared*eccSquared*eccSquared/1024)*sin(2*LatRad)
00143            + (15*eccSquared*eccSquared/256 + 45*eccSquared*eccSquared*eccSquared/1024)*sin(4*LatRad) 
00144            - (35*eccSquared*eccSquared*eccSquared/3072)*sin(6*LatRad))
00145     
00146     UTMEasting = (k0*N*(A+(1-T+C)*A*A*A/6
00147                         + (5-18*T+T*T+72*C-58*eccPrimeSquared)*A*A*A*A*A/120)
00148                   + 500000.0)
00149 
00150     UTMNorthing = (k0*(M+N*tan(LatRad)*(A*A/2+(5-T+9*C+4*C*C)*A*A*A*A/24
00151                                         + (61
00152                                            -58*T
00153                                            +T*T
00154                                            +600*C
00155                                            -330*eccPrimeSquared)*A*A*A*A*A*A/720)))
00156 
00157     if Lat < 0:
00158         UTMNorthing = UTMNorthing + 10000000.0; #10000000 meter offset for southern hemisphere
00159     return (UTMZone, UTMEasting, UTMNorthing)
00160 
00161 
00162 def _UTMLetterDesignator(Lat):
00163     if 84 >= Lat >= 72: return 'X'
00164     elif 72 > Lat >= 64: return 'W'
00165     elif 64 > Lat >= 56: return 'V'
00166     elif 56 > Lat >= 48: return 'U'
00167     elif 48 > Lat >= 40: return 'T'
00168     elif 40 > Lat >= 32: return 'S'
00169     elif 32 > Lat >= 24: return 'R'
00170     elif 24 > Lat >= 16: return 'Q'
00171     elif 16 > Lat >= 8: return 'P'
00172     elif  8 > Lat >= 0: return 'N'
00173     elif  0 > Lat >= -8: return 'M'
00174     elif -8> Lat >= -16: return 'L'
00175     elif -16 > Lat >= -24: return 'K'
00176     elif -24 > Lat >= -32: return 'J'
00177     elif -32 > Lat >= -40: return 'H'
00178     elif -40 > Lat >= -48: return 'G'
00179     elif -48 > Lat >= -56: return 'F'
00180     elif -56 > Lat >= -64: return 'E'
00181     elif -64 > Lat >= -72: return 'D'
00182     elif -72 > Lat >= -80: return 'C'
00183     else: return 'Z'    # if the Latitude is outside the UTM limits
00184 
00185 #void UTMtoLL(int ReferenceEllipsoid, const double UTMNorthing, const double UTMEasting, const char* UTMZone,
00186 #                         double& Lat,  double& Long )
00187 
00188 def UTMtoLL(ReferenceEllipsoid, northing, easting, zone):
00189     k0 = 0.9996
00190     a = _ellipsoid[ReferenceEllipsoid][_EquatorialRadius]
00191     eccSquared = _ellipsoid[ReferenceEllipsoid][_eccentricitySquared]
00192     e1 = (1-sqrt(1-eccSquared))/(1+sqrt(1-eccSquared))
00193     #NorthernHemisphere; //1 for northern hemispher, 0 for southern
00194 
00195     x = easting - 500000.0 #remove 500,000 meter offset for longitude
00196     y = northing
00197 
00198     ZoneLetter = zone[-1]
00199     ZoneNumber = int(zone[:-1])
00200     if ZoneLetter >= 'N':
00201         NorthernHemisphere = 1  # point is in northern hemisphere
00202     else:
00203         NorthernHemisphere = 0  # point is in southern hemisphere
00204         y -= 10000000.0         # remove 10,000,000 meter offset used for southern hemisphere
00205 
00206     LongOrigin = (ZoneNumber - 1)*6 - 180 + 3  # +3 puts origin in middle of zone
00207 
00208     eccPrimeSquared = (eccSquared)/(1-eccSquared)
00209 
00210     M = y / k0
00211     mu = M/(a*(1-eccSquared/4-3*eccSquared*eccSquared/64-5*eccSquared*eccSquared*eccSquared/256))
00212 
00213     phi1Rad = (mu + (3*e1/2-27*e1*e1*e1/32)*sin(2*mu) 
00214                + (21*e1*e1/16-55*e1*e1*e1*e1/32)*sin(4*mu)
00215                +(151*e1*e1*e1/96)*sin(6*mu))
00216     phi1 = phi1Rad*_rad2deg;
00217 
00218     N1 = a/sqrt(1-eccSquared*sin(phi1Rad)*sin(phi1Rad))
00219     T1 = tan(phi1Rad)*tan(phi1Rad)
00220     C1 = eccPrimeSquared*cos(phi1Rad)*cos(phi1Rad)
00221     R1 = a*(1-eccSquared)/pow(1-eccSquared*sin(phi1Rad)*sin(phi1Rad), 1.5)
00222     D = x/(N1*k0)
00223 
00224     Lat = phi1Rad - (N1*tan(phi1Rad)/R1)*(D*D/2-(5+3*T1+10*C1-4*C1*C1-9*eccPrimeSquared)*D*D*D*D/24
00225                                           +(61+90*T1+298*C1+45*T1*T1-252*eccPrimeSquared-3*C1*C1)*D*D*D*D*D*D/720)
00226     Lat = Lat * _rad2deg
00227 
00228     Long = (D-(1+2*T1+C1)*D*D*D/6+(5-2*C1+28*T1-3*C1*C1+8*eccPrimeSquared+24*T1*T1)
00229             *D*D*D*D*D/120)/cos(phi1Rad)
00230     Long = LongOrigin + Long * _rad2deg
00231     return (Lat, Long)
00232 
00233 if __name__ == '__main__':
00234     (z, e, n) = LLtoUTM(23, 45.00, -75.00)
00235     print z, e, n
00236     print UTMtoLL(23, e, n, z)
00237
gps_utm.py
