Mercurial Hosting > traffic-intelligence

annotate python/utils.py @ 24:6fb59cfb201e

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
first version of segmentIntersection
author Nicolas Saunier <nico@confins.net>
date Sat, 05 Dec 2009 11:04:03 -0500
parents 5a21d2cfee44
children 44689029a86f
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
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
9d6831cfe675 added tools for speed
Nicolas Saunier <nico@confins.net>
parents: 14
diff changeset
125 def printPoint(x,y):
9d6831cfe675 added tools for speed
Nicolas Saunier <nico@confins.net>
parents: 14
diff changeset
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
5a21d2cfee44 added polygon plotting
Nicolas Saunier <nico@confins.net>
parents: 16
diff changeset
128 def plotPolygon(poly, options = ''):
5a21d2cfee44 added polygon plotting
Nicolas Saunier <nico@confins.net>
parents: 16
diff changeset
129 from numpy.core.multiarray import array
5a21d2cfee44 added polygon plotting
Nicolas Saunier <nico@confins.net>
parents: 16
diff changeset
130 from matplotlib.pyplot import plot
5a21d2cfee44 added polygon plotting
Nicolas Saunier <nico@confins.net>
parents: 16
diff changeset
131 from shapely.geometry import Polygon
5a21d2cfee44 added polygon plotting
Nicolas Saunier <nico@confins.net>
parents: 16
diff changeset
132
5a21d2cfee44 added polygon plotting
Nicolas Saunier <nico@confins.net>
parents: 16
diff changeset
133 tmp = array(poly.exterior)
5a21d2cfee44 added polygon plotting
Nicolas Saunier <nico@confins.net>
parents: 16
diff changeset
134 plot(tmp[:,0], tmp[:,1], options)
5a21d2cfee44 added polygon plotting
Nicolas Saunier <nico@confins.net>
parents: 16
diff changeset
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")