Mercurial Hosting > traffic-intelligence

annotate python/utils.py @ 29:ca8e716cc231

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added moving average filter
author Nicolas Saunier <nico@confins.net>
date Sat, 30 Jan 2010 21:43:07 -0500
parents 44689029a86f
children c000f37c316d
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
0
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
1 #! /usr/bin/env python
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
2 ''' Generic utilities.'''
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
3
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
4 #from numpy import *
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
5 #from pylab import *
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
6
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
7 from moving import Point,Interval
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
8
0
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
9 __metaclass__ = type
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
10
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
11 commentChar = '#';
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
12
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
13 #########################
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
14 # maths section
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
15 #########################
24
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
16
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
17 def segmentIntersection(p1, p2, p3, p4):
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
18 '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise
24
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
19
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
20 >>> segmentIntersection(Point(0,0),Point(1,1), Point(0,1), Point(1,2))
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
21 >>> segmentIntersection(Point(0,1),Point(1,0), Point(0,2), Point(2,1))
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
22 >>> segmentIntersection(Point(0,0),Point(2,0), Point(1,-1),Point(1,1))
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
23 (1.000000,0.000000)
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
24 >>> segmentIntersection(Point(0,1),Point(2,0),Point(1,1),Point(1,2))
24
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
25 '''
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
26 from numpy import matrix
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
27 from numpy.linalg import linalg, det
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
28
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
29 dp1 = p2-p1#[s1[0][1]-s1[0][0], s1[1][1]-s1[1][0]]
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
30 dp2 = p4-p3#[s2[0][1]-s2[0][0], s2[1][1]-s2[1][0]]
24
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
31
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
32 A = matrix([[dp1.y, -dp1.x],
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
33 [dp2.y, -dp2.x]])
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
34 B = matrix([[dp1.y*p1.x-dp1.x*p1.y],
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
35 [dp2.y*p3.x-dp2.x*p3.y]])
24
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
36
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
37 if linalg.det(A) == 0:#crossProduct(ds1, ds2) == 0:
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
38 return None
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
39 else:
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
40 intersection = linalg.solve(A,B)
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
41 if (Interval(p1.x, p2.x, True).contains(intersection[0,0])
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
42 and Interval(p3.x, p4.x, True).contains(intersection[0,0])
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
43 and Interval(p1.y, p2.y, True).contains(intersection[1,0])
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
44 and Interval(p3.y, p4.y, True).contains(intersection[1,0])):
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
45 return Point(intersection[0,0], intersection[1,0])
24
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
46 else:
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
47 return None
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
48
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
49 def crossProduct(l1, l2):
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
50 return l1[0]*l2[1]-l1[1]*l2[0]
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
51
29
ca8e716cc231 added moving average filter
Nicolas Saunier <nico@confins.net>
parents: 27
diff changeset
52 def filterMovingWindow(input, halfWidth):
ca8e716cc231 added moving average filter
Nicolas Saunier <nico@confins.net>
parents: 27
diff changeset
53 '''Returns an array obtained after the smoothing of the input by a moving average
ca8e716cc231 added moving average filter
Nicolas Saunier <nico@confins.net>
parents: 27
diff changeset
54 The first and last points are copied from the original.'''
ca8e716cc231 added moving average filter
Nicolas Saunier <nico@confins.net>
parents: 27
diff changeset
55 width = float(halfWidth*2+1)
ca8e716cc231 added moving average filter
Nicolas Saunier <nico@confins.net>
parents: 27
diff changeset
56 win = ones(width,'d')
ca8e716cc231 added moving average filter
Nicolas Saunier <nico@confins.net>
parents: 27
diff changeset
57 result = convolve(win/width,array(inputSignal),'same')
ca8e716cc231 added moving average filter
Nicolas Saunier <nico@confins.net>
parents: 27
diff changeset
58 result[:halfWidth] = inputSignal[:halfWidth]
ca8e716cc231 added moving average filter
Nicolas Saunier <nico@confins.net>
parents: 27
diff changeset
59 result[-halfWidth:] = inputSignal[-halfWidth:]
ca8e716cc231 added moving average filter
Nicolas Saunier <nico@confins.net>
parents: 27
diff changeset
60 return result
ca8e716cc231 added moving average filter
Nicolas Saunier <nico@confins.net>
parents: 27
diff changeset
61
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
62 #########################
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
63 # file I/O section
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
64 #########################
24
6fb59cfb201e first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents: 19
diff changeset
65
0
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
66 def openCheck(filename, option = 'r', quit = False):
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
67 '''Open file filename in read mode by default
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
68 and checks it is open
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
69
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
70 >>> f = openCheck('non_existant_file.txt')
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
71 File non_existant_file.txt could not be opened.
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
72 '''
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
73 try:
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
74 return open(filename, option)
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
75 except IOError:
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
76 print 'File %s could not be opened.' % filename
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
77 if quit:
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
78 from sys import exit
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
79 exit()
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
80 return None
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
81
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
82 def readline(f):
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
83 '''Modified readline function to skip comments.'''
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
84 s = f.readline()
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
85 while (len(s) > 0) and s.startswith(commentChar):
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
86 s = f.readline()
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
87 return s.strip()
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
88
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
89 def removeExtension(filename, delimiter = '.'):
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
90 '''Returns the filename minus the extension (all characters after last .)
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
91 >>> removeExtension('test-adfasdf.asdfa.txt')
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
92 'test-adfasdf.asdfa'
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
93 >>> removeExtension('test-adfasdf')
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
94 'test-adfasdf'
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
95 '''
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
96 i = filename.rfind(delimiter)
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
97 if i>0:
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
98 return filename[:i]
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
99 else:
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
100 return filename
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
101
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
102 def listfiles(dirname, extension, remove = False):
14
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 7
diff changeset
103 '''Returns the list of files with the extension in the directory dirname
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 7
diff changeset
104 If remove is True, the filenames are stripped from the extension'''
0
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
105 from os import listdir
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
106 tmp = [f for f in listdir(dirname) if f.endswith(extension)]
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
107 tmp.sort()
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
108 if remove:
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
109 return [removeExtension(f, extension) for f in tmp]
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
110 else:
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
111 return tmp
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
112
14
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 7
diff changeset
113 def removeFile(filename):
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 7
diff changeset
114 '''Deletes the file while avoiding raising an error
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 7
diff changeset
115 if the file does not exist'''
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 7
diff changeset
116 if (os.path.exists(filename)):
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 7
diff changeset
117 os.remove(filename)
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 7
diff changeset
118
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 7
diff changeset
119 def invertHomography(homography):
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 7
diff changeset
120 'Returns an inverted homography'
27
44689029a86f updated segmentIntersection and other
Nicolas Saunier <nico@confins.net>
parents: 24
diff changeset
121 from numpy.linalg.linalg import inv
14
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 7
diff changeset
122 invH = inv(homography)
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 7
diff changeset
123 invH /= invH[2,2]
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 7
diff changeset
124 return invH
e7bbe8465591 homography and other utils
Nicolas Saunier <nico@confins.net>
parents: 7
diff changeset
125
19
5a21d2cfee44 added polygon plotting
Nicolas Saunier <nico@confins.net>
parents: 16
diff changeset
126 def plotPolygon(poly, options = ''):
5a21d2cfee44 added polygon plotting
Nicolas Saunier <nico@confins.net>
parents: 16
diff changeset
127 from numpy.core.multiarray import array
5a21d2cfee44 added polygon plotting
Nicolas Saunier <nico@confins.net>
parents: 16
diff changeset
128 from matplotlib.pyplot import plot
5a21d2cfee44 added polygon plotting
Nicolas Saunier <nico@confins.net>
parents: 16
diff changeset
129 from shapely.geometry import Polygon
5a21d2cfee44 added polygon plotting
Nicolas Saunier <nico@confins.net>
parents: 16
diff changeset
130
5a21d2cfee44 added polygon plotting
Nicolas Saunier <nico@confins.net>
parents: 16
diff changeset
131 tmp = array(poly.exterior)
5a21d2cfee44 added polygon plotting
Nicolas Saunier <nico@confins.net>
parents: 16
diff changeset
132 plot(tmp[:,0], tmp[:,1], options)
5a21d2cfee44 added polygon plotting
Nicolas Saunier <nico@confins.net>
parents: 16
diff changeset
133
0
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
134 if __name__ == "__main__":
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
135 import doctest
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
136 import unittest
7
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 2
diff changeset
137 #suite = doctest.DocFileSuite('tests/ubc_utils.txt')
ffddccfab7f9 loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents: 2
diff changeset
138 suite = doctest.DocTestSuite()
2
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
139 unittest.TextTestRunner().run(suite)
de5642925615 started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents: 0
diff changeset
140 #doctest.testmod()
0
aed8eb63cdde initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
141 #doctest.testfile("example.txt")