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) |
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 |