Path smoothing script for GPX files

gpxsmooth.py

import math
import numpy
import sys, os

from lxml import etree

Re = 6371000  # Earth radius in meters

# Extract the latitute and longitude as a tuple in radians from a <trkpt> element
def extract(trkpt) :
	return (math.pi / 180 * float(trkpt.attrib['lat']), math.pi / 180 * float(trkpt.attrib['lon']))

# Compute the great circle distance between two points given in polar
# coordinates and radians. The return value is in the same units as
# Re is defined.
def dist(p0, p1) :
	a = math.sin((p1[0] - p0[0])/2)**2 + math.cos(p0[0]) * math.cos(p1[0]) * math.sin((p1[1] - p0[1])/2)**2
	c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
	return Re * c

# Convert from polar to cartesian coordinates, radians to units of Re
def polcar(polarpt) :
	lat,lon = polarpt
	# Quick sanity check
	assert lat > -2*math.pi and lat < 2*math.pi
	assert lon > -2*math.pi and lon < 2*math.pi
	xyz = (math.cos(lat) * math.cos(lon), math.cos(lat) * math.sin(lon), math.sin(lat))
	return Re * numpy.array(xyz)

# Convert from cartesian to polar coordinates, radians to units of Re
def carpol(xyz) :
	R = numpy.linalg.norm(xyz)
	# Quick sanity check
	assert (R-Re)*1.0/Re < 1e-14

	lat = math.asin(numpy.dot(numpy.array([0,0,1]), xyz) / R)

	xy = xyz.copy()
	xy[2] = 0
	xy /= numpy.linalg.norm(xy)
	lon = math.atan2(xy[1], xy[0])

	return numpy.array( (lat,lon) )

# Given a pair of polar coordinates and a third, find the shortest great circle
# distance from the third point to the great circle arc segment connecting
# the first two.
def greatcircle_point_distance(pair, third) :
	# Convert to cartesian coordinates for the vector math
	cpair = tuple( map(lambda pt: polcar(numpy.array(pt)), pair) )
	cthird = polcar(numpy.array(third))

	# Project 'third' onto the great circle arc joining 'pair' along the
	# vector that is normal to the chord between 'pair'
	normal = numpy.cross(*cpair)
	normal /= numpy.linalg.norm(normal)
	intersect = cthird - normal * numpy.dot(normal, cthird)
	intersect *= Re / numpy.linalg.norm(intersect)

	# Great circle distance from 'third' to its projection
	d = dist(third, carpol(intersect))

	# If the projection of 'third' is not between the shorter arc
	# connecting 'pair', we instead want the gc distance from 'third'
	# to the nearest of the two.
	d0 = numpy.dot(intersect, cpair[0])
	d1 = numpy.dot(intersect, cpair[1])
	c = numpy.dot(numpy.cross(intersect, cpair[0]), numpy.cross(intersect, cpair[1]))

	if c < 0 and ((d0 >= 0 and d1 >= 0) or  (d0 < 0 and d1 < 0)) :
		return d
	else :
		return min((dist(third, pair[0]), dist(third, pair[1])))

# Examine a segment of gpx track and set 'keep' attributes on points
# as needed to stay within maxdistance and maxinterval.
#
# Given two kept trackpoints with no other kept points between them, the
# distance between the arc connecting these two points and any other
# trackpoints between them must be less than 'maxdistance'.
#
# Given two kept trackpoints, the distance between them should not be
# significantly greater than 'maxinterval'.
def process(seg, maxdistance = 5, maxinterval = 10) :
	bnds = (extract(seg[0]), extract(seg[-1]))

	# Find the point between this segment's endpoints that lies furthest
	# from the great circle arc between these endpoints
	idx,maxd = None,None
	for n in range(1, len(seg) - 1) :
		this = extract(seg[n])
		d = greatcircle_point_distance(bnds, this)
		if maxd is None or d > maxd :
			maxd = d
			idx = n

	# Python 3 fix. The following line was                                                          
	#   if maxd > maxdistance :                                                                 
	# and this gives a type error if maxd is None.                                              
	if maxd is not None and maxd  > maxdistance :
		# Keep this point if it is at least 'maxdistance' from the
		# connecting arc, and run 'process' on the two subsegments
		seg[idx].attrib['keep'] = 'True'
		process(seg[:idx], maxdistance, maxinterval)
		process(seg[idx:], maxdistance, maxinterval)
	# Python 3 fix. Following line was                                                          
	#   if maxinterval > 0 :                                                                 
	# and this gives a type error if maxd is None.                                              
	elif maxinterval is not None and maxinterval > 0 : #Python 3 fix                            
 		# This segment is good enough in terms of direction of travel.
		# A maximum distance between points may also be desired, however,
		# so loop through all remaining discarded points and add as needed.
		prev = bnds[0]
		fin = bnds[1]
		for pt in seg :
			this = extract(pt)
			if dist(prev, this) > maxinterval and dist(fin, this) > maxinterval :
				# Note that this does not satisfy the 'maxinterval' limit,
				# but instead takes the next point just further than the
				# given limit.
				# FIXME might be better to take the previous point, to make
				# the limit a guaranteed one.
				pt.attrib['keep'] = 'True'
				prev = this

	return seg

if __name__ == "__main__" :

	if len(sys.argv) < 4 :
		print "Usage: %s <gpx file> <maximum distance> <maximum interval>"%sys.argv[0]
		sys.exit(-1)

	file = os.path.splitext(sys.argv[1])
	assert file[1] == '.gpx' or file[1] == ''
	maxdistance = int(sys.argv[2])
	maxinterval = int(sys.argv[3])

	with open(file[0] + '.gpx', 'r') as fh :
		root = etree.parse(fh).getroot()
		NS = "{" + root.nsmap[None] + "}"

	# <gpx> contains a <trk> element, which contains a <trkseg> element
	# FIXME rewrite this to iterate over tracks and segments
	trkseg = root.find(NS + "trk").find(NS + "trkseg")

	# Add a 'keep' attribute to each <trkpt> element in the <trkseg>
	# Also remove elevation data
	print "Note: this script will also strip elevation data, assuming it to be inaccurate"
	for pt in trkseg :
		pt.attrib['keep'] = "False"
		for e in pt.iter(NS + "ele") :
			pt.remove(e)

	# We assume we need the first and last point
	# TODO make this configurable, to allow processing of only
	#      part of a track
	trkseg[0].attrib['keep'] = 'True'
	trkseg[-1].attrib['keep'] = 'True'

	process(trkseg, maxdistance, maxinterval)

	# Remove the unneeded trackpoints based on the 'keep' attribute
	m = len(trkseg)
	kill = filter(lambda pt: pt.attrib['keep'] == 'False', trkseg)
	# Python 3 fix. Added the following line, to convert kill to a list.                        
	killList= list(kill)
	# and now the following two lines refer to killList, not kill as originally.
	for n in range(0, len(killList)) :
		trkseg.remove(killList[n])
	n = len(trkseg)
	print "%d -> %d"%(m,n)

	# Remove the 'keep' attribute
	for n in range(0, len(trkseg)) :
		trkseg[n].attrib.pop('keep')

	# Write out the modified GPX file
	fh = open(file[0] + '_mod.gpx', 'w')
	# Python 3 fix. etree.tostring() returns bytes, which need to be converted to string with decode().                                                                            
	estr=(etree.tostring(root,  pretty_print=True, xml_declaration=True).decode())
	# Python 3 fix. The following line was originally                                          
	# xml = fh.write(etree.tostring(root), pretty_print=True, xml_declaration=True)             
	xml = fh.write(estr)
	fh.close()