Mercurial Hosting > traffic-intelligence
annotate python/utils.py @ 26:54d9cb0c902b
generalized intervals
author | Nicolas Saunier <nico@confins.net> |
---|---|
date | Sat, 05 Dec 2009 12:31:28 -0500 |
parents | 6fb59cfb201e |
children | 44689029a86f |
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 |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
7 __metaclass__ = type |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
8 |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
9 commentChar = '#'; |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
10 |
24
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
11 # maths |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
12 |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
13 def segmentIntersection(s1, s2): |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
14 '''Returns the intersecting point, None otherwise |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
15 format for a segment is [[x1, x2], [y1,y2]] |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
16 |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
17 >>> segmentIntersection([[0, 1], [0,1]], [[0, 1], [1,2]]) |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
18 >>> segmentIntersection([[0, 1], [1,0]], [[0, 1], [2,1]]) |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
19 >>> segmentIntersection([[0, 2], [0,0]], [[1, 1], [-1,1]]) |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
20 [1.0, 0.0] |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
21 >>> segmentIntersection([[0, 2], [0,0]], [[1, 1], [1,2]]) |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
22 ''' |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
23 from numpy import matrix |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
24 from numpy.linalg import linalg, det |
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 ds1 = [s1[0][1]-s1[0][0], s1[1][1]-s1[1][0]] |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
27 ds2 = [s2[0][1]-s2[0][0], s2[1][1]-s2[1][0]] |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
28 |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
29 A = matrix([[ds1[1], -ds1[0]], |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
30 [ds2[1], -ds2[0]]]) |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
31 B = matrix([[ds1[1]*s1[0][0]-ds1[0]*s1[1][0]], |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
32 [ds2[1]*s2[0][0]-ds2[0]*s2[1][0]]]) |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
33 |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
34 if linalg.det(A) == 0:#crossProduct(ds1, ds2) == 0: |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
35 return None |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
36 else: |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
37 intersection = linalg.solve(A,B) |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
38 if (intersection[0,0] >= s1[0][0] and intersection[0,0] <= s1[0][1] |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
39 and intersection[0,0] >= s2[0][0] and intersection[0,0] <= s2[0][1] |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
40 and intersection[1,0] >= s1[1][0] and intersection[1,0] <= s1[1][1] |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
41 and intersection[1,0] >= s2[1][0] and intersection[1,0] <= s2[1][1]): |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
42 return [intersection[0,0], intersection[1,0]] |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
43 else: |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
44 return None |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
45 |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
46 def crossProduct(l1, l2): |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
47 return l1[0]*l2[1]-l1[1]*l2[0] |
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 # file I/O |
6fb59cfb201e
first version of segmentIntersection
Nicolas Saunier <nico@confins.net>
parents:
19
diff
changeset
|
50 |
0
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
51 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
|
52 '''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
|
53 and checks it is open |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
54 |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
55 >>> f = openCheck('non_existant_file.txt') |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
56 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
|
57 ''' |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
58 try: |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
59 return open(filename, option) |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
60 except IOError: |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
61 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
|
62 if quit: |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
63 from sys import exit |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
64 exit() |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
65 return None |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
66 |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
67 def readline(f): |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
68 '''Modified readline function to skip comments.''' |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
69 s = f.readline() |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
70 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
|
71 s = f.readline() |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
72 return s.strip() |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
73 |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
74 def removeExtension(filename, delimiter = '.'): |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
75 '''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
|
76 >>> removeExtension('test-adfasdf.asdfa.txt') |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
77 'test-adfasdf.asdfa' |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
78 >>> removeExtension('test-adfasdf') |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
79 'test-adfasdf' |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
80 ''' |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
81 i = filename.rfind(delimiter) |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
82 if i>0: |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
83 return filename[:i] |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
84 else: |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
85 return filename |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
86 |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
87 def listfiles(dirname, extension, remove = False): |
14
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
88 '''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
|
89 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
|
90 from os import listdir |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
91 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
|
92 tmp.sort() |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
93 if remove: |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
94 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
|
95 else: |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
96 return tmp |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
97 |
14
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
98 def removeFile(filename): |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
99 '''Deletes the file while avoiding raising an error |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
100 if the file does not exist''' |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
101 if (os.path.exists(filename)): |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
102 os.remove(filename) |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
103 |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
104 def invertHomography(homography): |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
105 'Returns an inverted homography' |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
106 invH = inv(homography) |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
107 invH /= invH[2,2] |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
108 return invH |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
109 |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
110 def project(homography, p): |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
111 '''Returns the coordinates of the projection of the point p |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
112 through homography''' |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
113 from numpy.core._dotblas import dot |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
114 from numpy.core.multiarray import array |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
115 from numpy.lib.function_base import insert |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
116 if (homography!=None) and (len(homography)>0): |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
117 pAugmented = insert(array(p), [2],[1], axis=0) |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
118 projected = dot(homography, pAugmented) |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
119 projected[0] /= projected[2] |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
120 projected[1] /= projected[2] |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
121 else: |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
122 projected = p |
e7bbe8465591
homography and other utils
Nicolas Saunier <nico@confins.net>
parents:
7
diff
changeset
|
123 return projected[:2] |
0
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
124 |
16 | 125 def printPoint(x,y): |
126 return '(%f,%f)'%(x,y) | |
0
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
127 |
19 | 128 def plotPolygon(poly, options = ''): |
129 from numpy.core.multiarray import array | |
130 from matplotlib.pyplot import plot | |
131 from shapely.geometry import Polygon | |
132 | |
133 tmp = array(poly.exterior) | |
134 plot(tmp[:,0], tmp[:,1], options) | |
135 | |
0
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
136 if __name__ == "__main__": |
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
137 import doctest |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
138 import unittest |
7
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
2
diff
changeset
|
139 #suite = doctest.DocFileSuite('tests/ubc_utils.txt') |
ffddccfab7f9
loading shell objects from NGSIM works
Nicolas Saunier <nico@confins.net>
parents:
2
diff
changeset
|
140 suite = doctest.DocTestSuite() |
2
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
141 unittest.TextTestRunner().run(suite) |
de5642925615
started implementation of TimeInterval and Spatio-temporal object
Nicolas Saunier <nicolas.saunier@polymtl.ca>
parents:
0
diff
changeset
|
142 #doctest.testmod() |
0
aed8eb63cdde
initial commit with non-functional python code for NGSIM
Nicolas Saunier <nico@confins.net>
parents:
diff
changeset
|
143 #doctest.testfile("example.txt") |