Advertisement
Guest User

Untitled

a guest
Nov 28th, 2011
35
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 3.15 KB | None | 0 0
  1. #!/usr/bin/env python
  2. # -*- coding: utf8 -*-
  3.  
  4. import sys
  5. from math import *
  6. from lxml import etree
  7.  
  8. rE = 6356752.314245 # earth's radius
  9.  
  10. def rotate(x, y, phi):
  11.     return x*cos(phi) - y*sin(phi), x*sin(phi) + y*cos(phi)
  12.  
  13. def latlon_to_xyz(lat, lon):
  14.     lat = radians(lat)
  15.     lon = radians(lon)
  16.     return cos(lat)*sin(lon), -cos(lat)*cos(lon), sin(lat)
  17.  
  18. def project_to_meters(lat, lon, latm, lonm):
  19.     # azimuthal map projection centered at average track coordinate
  20.     lon -= lonm
  21.     xyz = latlon_to_xyz(lat, lon)
  22.     zy = rotate(xyz[2], xyz[1], radians(90 - latm))
  23.     lat2 = atan2(zy[0], sqrt(zy[1]**2 + xyz[0]**2))
  24.     lon2 = atan2(xyz[0], -zy[1])
  25.  
  26.     x_meters = rE * sin(lon2) * (pi / 2.0 - lat2)
  27.     y_meters = -rE * cos(lon2) * (pi / 2.0 - lat2)
  28.     return x_meters, y_meters
  29.  
  30. projection = {}
  31. def get_dist(latlon1, latlon2):
  32.     p1, p2 = projection[latlon1], projection[latlon2]
  33.     return sqrt((p1[0] - p2[0]) ** 2 + (p1[1] - p2[1]) ** 2)
  34.  
  35. tree = etree.parse(sys.stdin)
  36. gpx = tree.getroot()
  37.  
  38. if gpx.nsmap.has_key(None):
  39.     nsmap = '{' + gpx.nsmap[None] + '}'
  40. else:
  41.     nsmap = ''
  42.  
  43. trk = gpx.find(nsmap + 'trk')
  44. trksegs = trk.findall(nsmap + 'trkseg')
  45.  
  46. # prepare projection
  47. base = None
  48. for trkseg in trksegs:
  49.     trkpts = trkseg.findall(nsmap + 'trkpt')
  50.     for trkpt in trkpts:
  51.         latlon = float(trkpt.get('lat')), float(trkpt.get('lon'))
  52.         if base is None:
  53.             base = latlon
  54.        
  55.         projection[latlon] = project_to_meters(latlon[0], latlon[1], base[0], base[1])
  56.  
  57. # read points into segments
  58. clusters = []
  59. segments = []
  60. for trkseg in trksegs:
  61.     trkpts = trkseg.findall(nsmap + 'trkpt')
  62.     segment = []
  63.     for trkpt in trkpts:
  64.         latlon = float(trkpt.get('lat')), float(trkpt.get('lon'))
  65.         segment.append(latlon)
  66.     segments.append(segment)
  67.    
  68. # For each segment, consider whether it's OK to drop it.
  69. # It _is_ OK to drop it if 80% of its points are in 5m distance from
  70. # points from at least 3 other segments
  71. kept_segments = []
  72. for idx, segment in enumerate(segments):
  73.     #print idx, len(segments)
  74.     redundant_count = 0
  75.     for latlon in segment:
  76.         neighbours = 0
  77.         for other_segment in kept_segments:
  78.             if other_segment == segment: continue
  79.             for other_latlon in other_segment:
  80.                 d = get_dist(latlon, other_latlon)
  81.                 if d < 5: # closer than 5m
  82.                     neighbours += 1
  83.                     break
  84.                 if d > 2000:
  85.                     # This is likely too far away
  86.                     break
  87.             if neighbours == 3: break # we've found enough!                
  88.         if neighbours == 3:
  89.             redundant_count += 1
  90.     if redundant_count > len(segment) * 0.8:
  91.         # 80% of points are redundant, remove!
  92.         pass
  93.     else:
  94.         kept_segments.append(segment)
  95.  
  96. print "<?xml version='1.0' encoding='UTF-8'?><gpx version='1.0'><name>car gpx_d-end</name><trk>"
  97. for segment in kept_segments:
  98.     print "<trkseg>"
  99.     for lat, lon in segment:
  100.         print '<trkpt lat="%f" lon="%f"></trkpt>' % (lat, lon)
  101.     print "</trkseg>"
  102. print "</trk></gpx>"
  103.  
  104.  
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement