gpx_reduce.py
changeset 2 f93786d4f68e
parent 1 5467c893d121
child 3 f94099a0277a
equal deleted inserted replaced
1:5467c893d121 2:f93786d4f68e
    29 '''
    29 '''
    30 
    30 
    31 import datetime
    31 import datetime
    32 import sys
    32 import sys
    33 import time
    33 import time
    34 import numpy as sc
    34 import numpy as np
    35 import numpy.linalg as la
    35 import numpy.linalg as la
    36 from math import *
    36 from math import *
    37 from lxml import etree
    37 import xml.etree.ElementTree as etree
    38 from optparse import OptionParser
    38 from optparse import OptionParser
    39 
    39 
    40 
    40 
    41 parser = OptionParser('usage: %prog [options] input-file.gpx')
    41 parser = OptionParser('usage: %prog [options] input-file.gpx')
    42 parser.add_option('-v', '--verbose', action='store', type='int',
    42 parser.add_option('-v', '--verbose', action='store', type='int',
    84 
    84 
    85 
    85 
    86 def distance(p1_, pm_, p2_, ele_weight=1.0):
    86 def distance(p1_, pm_, p2_, ele_weight=1.0):
    87     # returns distance of pm from line between p1 and p2
    87     # returns distance of pm from line between p1 and p2
    88 
    88 
    89     p1, pm, p2 = sc.array(p1_), sc.array(pm_), sc.array(p2_)
    89     p1, pm, p2 = np.array(p1_), np.array(pm_), np.array(p2_)
    90     h1, hm, h2 = la.norm(p1), la.norm(pm), la.norm(p2)
    90     h1, hm, h2 = la.norm(p1), la.norm(pm), la.norm(p2)
    91     if ele_weight != 1.0 and min(h1, hm, h2) > 0.0:
    91     if ele_weight != 1.0 and min(h1, hm, h2) > 0.0:
    92         hmean = (h1 + hm + h2) / 3.0
    92         hmean = (h1 + hm + h2) / 3.0
    93         p1 *= (ele_weight + (1.0 - ele_weight) * hmean / h1)
    93         p1 *= (ele_weight + (1.0 - ele_weight) * hmean / h1)
    94         pm *= (ele_weight + (1.0 - ele_weight) * hmean / hm)
    94         pm *= (ele_weight + (1.0 - ele_weight) * hmean / hm)
    98     vm = pm - p1
    98     vm = pm - p1
    99     if linel == 0.0:
    99     if linel == 0.0:
   100         return la.norm(vm)
   100         return la.norm(vm)
   101     linem = line / linel
   101     linem = line / linel
   102     
   102     
   103     position = sc.dot(vm, linem) / linel
   103     position = np.dot(vm, linem) / linel
   104     if position < 0.0:
   104     if position < 0.0:
   105         return la.norm(vm)
   105         return la.norm(vm)
   106     elif position > 1.0:
   106     elif position > 1.0:
   107         return la.norm(pm - p2)
   107         return la.norm(pm - p2)
   108     else:
   108     else:
   179     for i2 in range(1, n):
   179     for i2 in range(1, n):
   180         penalties = {}
   180         penalties = {}
   181         imin = None
   181         imin = None
   182         costmin = float('inf')
   182         costmin = float('inf')
   183         for i1 in reversed(range(i2)):
   183         for i1 in reversed(range(i2)):
   184             p1 = sc.array(points[i1]['p'])
   184             p1 = np.array(points[i1]['p'])
   185             p2 = sc.array(points[i2]['p'])
   185             p2 = np.array(points[i2]['p'])
   186             seglength = la.norm(p2 - p1)
   186             seglength = la.norm(p2 - p1)
   187             
   187             
   188             # estimate speed between p1 and p2
   188             # estimate speed between p1 and p2
   189             if timesteps != None:
   189             if timesteps != None:
   190                 dt = (points[i2]['t'] - points[i1]['t']).total_seconds()
   190                 dt = (points[i2]['t'] - points[i1]['t']).total_seconds()
   214             distance_squaremax = 0.0
   214             distance_squaremax = 0.0
   215             distance_squaresum = 0.0
   215             distance_squaresum = 0.0
   216             distances_squared = []
   216             distances_squared = []
   217             # iterate all medium points between i1 and i2
   217             # iterate all medium points between i1 and i2
   218             for im in range(i1+1, i2):
   218             for im in range(i1+1, i2):
   219                 pm = sc.array(points[im]['p'])
   219                 pm = np.array(points[im]['p'])
   220                 d = distance(p1, pm, p2, options.ele_weight)
   220                 d = distance(p1, pm, p2, options.ele_weight)
   221                 if (d <= options.max_dist):
   221                 if (d <= options.max_dist):
   222                     d_sq = (d / options.max_dist) ** 2
   222                     d_sq = (d / options.max_dist) ** 2
   223                     distance_squaremax = max(distance_squaremax, d_sq)
   223                     distance_squaremax = max(distance_squaremax, d_sq)
   224                     distance_squaresum += points[im]['weight'] * d_sq
   224                     distance_squaresum += points[im]['weight'] * d_sq
   225                     distances_squared.append(d_sq)
   225                     distances_squared.append(d_sq)
   226                 else:
   226                 else:
   227                     i1_i2_segment_valid = False
   227                     i1_i2_segment_valid = False
   228                 
   228                 
   229                     # check if connection to any further point i1 is impossible
   229                     # check if connection to any further point i1 is impossible
   230                     d1 = sc.dot(p1 - p2, p1 - p2)
   230                     d1 = np.dot(p1 - p2, p1 - p2)
   231                     d2 = sc.dot(pm - p2, pm - p2)
   231                     d2 = np.dot(pm - p2, pm - p2)
   232                     dd = options.max_dist ** 2
   232                     dd = options.max_dist ** 2
   233                     d1d2 = sc.dot(p1 - p2, pm - p2)
   233                     d1d2 = np.dot(p1 - p2, pm - p2)
   234                     # formula from cosines of point separation angle and cone-opening angles around points
   234                     # formula from cosines of point separation angle and cone-opening angles around points
   235                     if (d1 > dd and d2 > dd and (d1d2 + dd)**2 < (d2 - dd) * (d1 - dd)):
   235                     if (d1 > dd and d2 > dd and (d1d2 + dd)**2 < (d2 - dd) * (d1 - dd)):
   236                         lower_i1_possible = False
   236                         lower_i1_possible = False
   237                         break
   237                         break
   238             
   238             
   255                     penalties[i1] = 0.0
   255                     penalties[i1] = 0.0
   256                 
   256                 
   257                 # add a penalty for kinks
   257                 # add a penalty for kinks
   258                 if options.bend > 0.:
   258                 if options.bend > 0.:
   259                     if points[i1]['prev'] != -1:
   259                     if points[i1]['prev'] != -1:
   260                         p0 = sc.array(points[points[i1]['prev']]['p'])
   260                         p0 = np.array(points[points[i1]['prev']]['p'])
   261                         v0 = p1 - p0
   261                         v0 = p1 - p0
   262                         v1 = p2 - p1
   262                         v1 = p2 - p1
   263                         if la.norm(v0) > 0. and la.norm(v1) > 0.:
   263                         if la.norm(v0) > 0. and la.norm(v1) > 0.:
   264                             v0 /= la.norm(v0)
   264                             v0 /= la.norm(v0)
   265                             v1 /= la.norm(v1)
   265                             v1 /= la.norm(v1)
   266                             kink = (1.0 - sc.dot(v0, v1)) / 2.0
   266                             kink = (1.0 - np.dot(v0, v1)) / 2.0
   267                             penalties[i1] += options.bend * kink
   267                             penalties[i1] += options.bend * kink
   268         
   268         
   269         # find best predecessor
   269         # find best predecessor
   270         imin = None
   270         imin = None
   271         costmin = float('inf')
   271         costmin = float('inf')
   308     sumx, sumy, sumz = 0.0, 0.0, 0.0
   308     sumx, sumy, sumz = 0.0, 0.0, 0.0
   309     
   309     
   310     # import xml data from files
   310     # import xml data from files
   311     print 'opening file', fname
   311     print 'opening file', fname
   312     infile = open(fname)
   312     infile = open(fname)
   313     
       
   314     tree = etree.parse(infile)
   313     tree = etree.parse(infile)
   315     infile.close()
   314     infile.close()
       
   315 
   316     gpx = tree.getroot()
   316     gpx = tree.getroot()
   317     if gpx.nsmap.has_key(None):
   317     nsurl = gpx.tag.split ('}')[0][1:]  # == 'http://www.topografix.com/GPX/1/1'
   318         nsmap = '{' + gpx.nsmap[None] + '}'
   318     etree.register_namespace('', nsurl) # default namespace -> xmlns:.... in the output
   319     else:
   319     nsmap = '{' + nsurl + '}'           # prefix for all tags in the tree
   320         nsmap = ''
   320 
   321     
       
   322     # extract data from xml
   321     # extract data from xml
   323     for trkseg in gpx.findall('.//' + nsmap + 'trkseg'):
   322     for trkseg in gpx.findall('.//' + nsmap + 'trkseg'):
   324         trkpts = trkseg.findall(nsmap + 'trkpt')
   323         trkpts = trkseg.findall(nsmap + 'trkpt')
   325         n = len(trkpts)
   324         n = len(trkpts)
   326         
   325         
   355         print 'number of points:', n, '-', n - n_new, '=', n_new
   354         print 'number of points:', n, '-', n - n_new, '=', n_new
   356         
   355         
   357         # delete certain points from original data
   356         # delete certain points from original data
   358         delete_pnums = [i for i in range(n) if i not in final_pnums]
   357         delete_pnums = [i for i in range(n) if i not in final_pnums]
   359         for i in reversed(delete_pnums):
   358         for i in reversed(delete_pnums):
   360             del trkseg[trkseg.index(trkpts[i])]
   359             trkseg.remove (trkpts[i])
   361         
   360         
   362         # save reduced trackseg for plotting
   361         # save reduced trackseg for plotting
   363         if options.plot:
   362         if options.plot:
   364             tracksegs_new.append([[float(trkpt.get('lat')),
   363             tracksegs_new.append([[float(trkpt.get('lat')),
   365                 float(trkpt.get('lon')), float(trkpt.find(nsmap + 'ele').text)]
   364                 float(trkpt.get('lon')), float(trkpt.find(nsmap + 'ele').text)]
   372     elif fname.endswith('.gpx'):
   371     elif fname.endswith('.gpx'):
   373         ofname = fname[:-4] + '_reduced.gpx'
   372         ofname = fname[:-4] + '_reduced.gpx'
   374     else:
   373     else:
   375         ofname = fname + '_reduced.gpx'
   374         ofname = fname + '_reduced.gpx'
   376     outfile = open(ofname, 'w')
   375     outfile = open(ofname, 'w')
   377     outfile.write(etree.tostring(tree, xml_declaration=True,
   376     tree.write (outfile, encoding="utf-8", xml_declaration=True, default_namespace=None, method="xml")
   378         pretty_print=True, encoding='utf-8'))
       
   379     outfile.close()
   377     outfile.close()
   380     print 'modified copy written to', ofname
   378     print 'modified copy written to', ofname
   381     
   379 
   382     
       
   383     # plot result to screen
   380     # plot result to screen
   384     if options.plot:
   381     if options.plot:
   385         import pylab as pl
   382         import pylab as pl
   386         latm, lonm, elesum = xyz_to_latlonele(sumx, sumy, sumz)
   383         latm, lonm, elesum = xyz_to_latlonele(sumx, sumy, sumz)
   387 
   384