repo/traffic-intelligence: python/moving.py annotate

annotate python/moving.py @ 578:fe4e9d2b807d

finalizing transformcoordinates for each object

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Thu, 28 Aug 2014 17:42:32 -0400
parents	d0abd2ee17b9
children	05c927c6d3cf

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 '''Libraries for moving objects, trajectories...'''
aed8eb63cdde initial commit with non-functional python code for NGSIM Nicolas Saunier <nico@confins.net> parents: diff changeset	3
258 d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	4 import utils
d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	5 import cvutils
0 aed8eb63cdde initial commit with non-functional python code for NGSIM Nicolas Saunier <nico@confins.net> parents: diff changeset	6
258 d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	7 from math import sqrt
345 fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	8 from numpy import median
16 9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	9
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	10 try:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	11 from shapely.geometry import Polygon, Point as shapelyPoint
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	12 from shapely.prepared import prep
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	13 shapelyAvailable = True
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	14 except ImportError:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	15 print('Shapely library could not be loaded')
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	16 shapelyAvailable = False
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	17
0 aed8eb63cdde initial commit with non-functional python code for NGSIM Nicolas Saunier <nico@confins.net> parents: diff changeset	18 __metaclass__ = type
aed8eb63cdde initial commit with non-functional python code for NGSIM Nicolas Saunier <nico@confins.net> parents: diff changeset	19
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	20 class Interval(object):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	21 '''Generic interval: a subset of real numbers (not iterable)'''
26 54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	22 def __init__(self, first=0, last=-1, revert = False):
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	23 if revert and last<first:
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	24 self.first=last
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	25 self.last=first
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	26 else:
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	27 self.first=first
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	28 self.last=last
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	29
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	30 def __str__(self):
104 13187af8622d finally added the representation of intervals Nicolas Saunier <nico@confins.net> parents: 98 diff changeset	31 return '[{0}, {1}]'.format(self.first, self.last)
13187af8622d finally added the representation of intervals Nicolas Saunier <nico@confins.net> parents: 98 diff changeset	32
13187af8622d finally added the representation of intervals Nicolas Saunier <nico@confins.net> parents: 98 diff changeset	33 def __repr__(self):
13187af8622d finally added the representation of intervals Nicolas Saunier <nico@confins.net> parents: 98 diff changeset	34 return self.__str__()
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	35
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	36 def empty(self):
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	37 return self.first > self.last
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	38
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	39 def center(self):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	40 return (self.first+self.last)/2.
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	41
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	42 def length(self):
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	43 '''Returns the length of the interval'''
91 daa05fae1a70 modified the type of the result of interval lengths to float, added comments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 90 diff changeset	44 return float(max(0,self.last-self.first))
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	45
96 9928c2fa72cc added equal method to intervals Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 93 diff changeset	46 def equal(self, i2):
9928c2fa72cc added equal method to intervals Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 93 diff changeset	47 return self.first==i2.first and self.last == i2.last
9928c2fa72cc added equal method to intervals Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 93 diff changeset	48
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	49 def getList(self):
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	50 return [self.first, self.last]
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	51
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	52 def contains(self, instant):
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	53 return (self.first<=instant and self.last>=instant)
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	54
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	55 def inside(self, interval2):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	56 '''Indicates if the temporal interval of self is comprised in interval2'''
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	57 return (self.first >= interval2.first) and (self.last <= interval2.last)
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	58
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	59 @classmethod
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	60 def union(cls, interval1, interval2):
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	61 '''Smallest interval comprising self and interval2'''
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	62 return cls(min(interval1.first, interval2.first), max(interval2.last, interval2.last))
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	63
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	64 @classmethod
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	65 def intersection(cls, interval1, interval2):
104 13187af8622d finally added the representation of intervals Nicolas Saunier <nico@confins.net> parents: 98 diff changeset	66 '''Largest interval comprised in both self and interval2'''
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	67 return cls(max(interval1.first, interval2.first), min(interval1.last, interval2.last))
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	68
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	69 def distance(self, interval2):
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	70 if not Interval.intersection(self, interval2).empty():
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	71 return 0
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	72 elif self.first > interval2.last:
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	73 return self.first - interval2.last
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	74 elif self.last < interval2.first:
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	75 return interval2.first - self.last
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	76 else:
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	77 return None
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	78
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	79
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	80 def unionIntervals(intervals):
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	81 'returns the smallest interval containing all intervals'
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	82 inter = intervals[0]
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	83 for i in intervals[1:]:
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	84 inter = Interval.union(inter, i)
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	85 return inter
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	86
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	87
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	88 class TimeInterval(Interval):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	89 '''Temporal interval: set of instants at fixed time step, between first and last, included
e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	90
e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	91 For example: based on frame numbers (hence the modified length method)
e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	92 It may be modified directly by setting first and last'''
26 54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	93
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	94 def __init__(self, first=0, last=-1):
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	95 super(TimeInterval, self).__init__(first, last, False)
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	96
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	97 @staticmethod
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	98 def fromInterval(inter):
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	99 return TimeInterval(inter.first, inter.last)
26 54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	100
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	101 def __getitem__(self, i):
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	102 if not self.empty():
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	103 if isinstance(i, int):
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	104 return self.first+i
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	105 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	106 raise TypeError, "Invalid argument type."
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	107 #elif isinstance( key, slice ):
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	108
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	109 def __iter__(self):
107 916678481896 corrected bug for TimeInterval interation and added corresponding test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 105 diff changeset	110 self.iterInstantNum = -1
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	111 return self
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	112
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	113 def next(self):
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	114 if self.iterInstantNum >= self.length()-1:
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	115 raise StopIteration
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	116 else:
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	117 self.iterInstantNum += 1
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	118 return self[self.iterInstantNum]
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	119
26 54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	120 def length(self):
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	121 '''Returns the length of the interval'''
91 daa05fae1a70 modified the type of the result of interval lengths to float, added comments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 90 diff changeset	122 return float(max(0,self.last-self.first+1))
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	123
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	124 # class BoundingPolygon:
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	125 # '''Class for a polygon bounding a set of points
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	126 # with methods to create intersection, unions...
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	127 # '''
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	128 # We will use the polygon class of Shapely
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	129
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	130 class STObject(object):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	131 '''Class for spatio-temporal object, i.e. with temporal and spatial existence
6 597d61c1eebe minor doc update Nicolas Saunier <nico@confins.net> parents: 2 diff changeset	132 (time interval and bounding polygon for positions (e.g. rectangle)).
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	133
e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	134 It may not mean that the object is defined
6 597d61c1eebe minor doc update Nicolas Saunier <nico@confins.net> parents: 2 diff changeset	135 for all time instants within the time interval'''
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	136
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	137 def __init__(self, num = None, timeInterval = None, boundingPolygon = None):
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	138 self.num = num
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	139 self.timeInterval = timeInterval
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	140 self.boundingPolygon = boundingPolygon
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	141
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	142 def empty(self):
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	143 return self.timeInterval.empty() or not self.boudingPolygon
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	144
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	145 def getNum(self):
211 ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	146 return self.num
ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	147
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	148 def getFirstInstant(self):
40 9f16aee24b7e corrected bug for first and last of TimeInterval Nicolas Saunier <nico@confins.net> parents: 39 diff changeset	149 return self.timeInterval.first
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	150
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	151 def getLastInstant(self):
40 9f16aee24b7e corrected bug for first and last of TimeInterval Nicolas Saunier <nico@confins.net> parents: 39 diff changeset	152 return self.timeInterval.last
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	153
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	154 def getTimeInterval(self):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	155 return self.timeInterval
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	156
108 6efe470ea5e5 added test existsAtInstant to STObject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 107 diff changeset	157 def existsAtInstant(self, t):
6efe470ea5e5 added test existsAtInstant to STObject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 107 diff changeset	158 return self.timeInterval.contains(t)
6efe470ea5e5 added test existsAtInstant to STObject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 107 diff changeset	159
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	160 def commonTimeInterval(self, obj2):
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	161 return TimeInterval.intersection(self.getTimeInterval(), obj2.getTimeInterval())
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	162
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	163 class Point(object):
25 28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	164 def __init__(self, x, y):
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	165 self.x = x
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	166 self.y = y
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	167
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	168 def __str__(self):
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	169 return '(%f,%f)'%(self.x,self.y)
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	170
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	171 def __repr__(self):
98 b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	172 return self.__str__()
25 28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	173
184 d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	174 def __add__(self, other):
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	175 return Point(self.x+other.x, self.y+other.y)
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	176
25 28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	177 def __sub__(self, other):
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	178 return Point(self.x-other.x, self.y-other.y)
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	179
451 cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	180 def __neg__(self):
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	181 return Point(-self.x, -self.y)
575 13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	182
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	183 def __getitem__(self, i):
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	184 if i == 0:
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	185 return self.x
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	186 elif i == 1:
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	187 return self.y
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	188 else:
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	189 raise IndexError()
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	190
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	191 def orthogonal(self):
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	192 return Point(self.y, -self.x)
451 cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	193
105 9844c69d8fa2 added multiply method to Point Nicolas Saunier <nico@confins.net> parents: 104 diff changeset	194 def multiply(self, alpha):
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	195 'Warning, returns a new Point'
105 9844c69d8fa2 added multiply method to Point Nicolas Saunier <nico@confins.net> parents: 104 diff changeset	196 return Point(self.xalpha, self.yalpha)
9844c69d8fa2 added multiply method to Point Nicolas Saunier <nico@confins.net> parents: 104 diff changeset	197
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	198 def plot(self, options = 'o', **kwargs):
41 eb78c6edc0c8 added drawing for Point Nicolas Saunier <nico@confins.net> parents: 40 diff changeset	199 from matplotlib.pylab import plot
211 ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	200 plot([self.x], [self.y], options, **kwargs)
41 eb78c6edc0c8 added drawing for Point Nicolas Saunier <nico@confins.net> parents: 40 diff changeset	201
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	202 def norm2Squared(self):
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	203 '''2-norm distance (Euclidean distance)'''
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	204 return self.x2+self.y2
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	205
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	206 def norm2(self):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	207 '''2-norm distance (Euclidean distance)'''
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	208 return sqrt(self.norm2Squared())
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	209
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	210 def norm1(self):
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	211 return abs(self.x)+abs(self.y)
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	212
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	213 def normMax(self):
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	214 return max(abs(self.x),abs(self.y))
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	215
64 c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	216 def aslist(self):
c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	217 return [self.x, self.y]
c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	218
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	219 def astuple(self):
e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	220 return (self.x, self.y)
e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	221
223 c31722fcc9de minor modifications for integer drawing in OpenCV Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 214 diff changeset	222 def asint(self):
c31722fcc9de minor modifications for integer drawing in OpenCV Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 214 diff changeset	223 return Point(int(self.x), int(self.y))
c31722fcc9de minor modifications for integer drawing in OpenCV Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 214 diff changeset	224
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	225 if shapelyAvailable:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	226 def asShapely(self):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	227 return shapelyPoint(self.x, self.y)
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	228
98 b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	229 def project(self, homography):
b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	230 from numpy.core.multiarray import array
b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	231 projected = cvutils.projectArray(homography, array([[self.x], [self.y]]))
b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	232 return Point(projected[0], projected[1])
b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	233
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	234 def inPolygonNoShapely(self, polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	235 '''Indicates if the point x, y is inside the polygon
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	236 (array of Nx2 coordinates of the polygon vertices)
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	237
92 a5ef9e40688e makes use of matplotlib function to test if point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 91 diff changeset	238 taken from http://www.ariel.com.au/a/python-point-int-poly.html
a5ef9e40688e makes use of matplotlib function to test if point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 91 diff changeset	239
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	240 Use Polygon.contains if Shapely is installed'''
79 5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	241
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	242 n = polygon.shape[0];
79 5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	243 counter = 0;
5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	244
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	245 p1 = polygon[0,:];
79 5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	246 for i in range(n+1):
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	247 p2 = polygon[i % n,:];
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	248 if self.y > min(p1[1],p2[1]):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	249 if self.y <= max(p1[1],p2[1]):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	250 if self.x <= max(p1[0],p2[0]):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	251 if p1[1] != p2[1]:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	252 xinters = (self.y-p1[1])*(p2[0]-p1[0])/(p2[1]-p1[1])+p1[0];
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	253 if p1[0] == p2[0] or self.x <= xinters:
79 5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	254 counter+=1;
5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	255 p1=p2
5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	256 return (counter%2 == 1);
5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	257
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	258 @staticmethod
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	259 def fromList(p):
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	260 return Point(p[0], p[1])
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	261
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	262 @staticmethod
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	263 def dot(p1, p2):
89 f88a19695bba added inner product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 83 diff changeset	264 'Scalar product'
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	265 return p1.xp2.x+p1.yp2.y
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	266
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	267 @staticmethod
90 f84293ad4611 renamed inner to cross product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 89 diff changeset	268 def cross(p1, p2):
f84293ad4611 renamed inner to cross product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 89 diff changeset	269 'Cross product'
89 f88a19695bba added inner product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 83 diff changeset	270 return p1.xp2.y-p1.yp2.x
f88a19695bba added inner product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 83 diff changeset	271
f88a19695bba added inner product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 83 diff changeset	272 @staticmethod
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	273 def cosine(p1, p2):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	274 return Point.dot(p1,p2)/(p1.norm2()*p2.norm2())
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	275
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	276 @staticmethod
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	277 def distanceNorm2(p1, p2):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	278 return (p1-p2).norm2()
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	279
64 c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	280 @staticmethod
357 e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 345 diff changeset	281 def plotAll(points, **kwargs):
64 c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	282 from matplotlib.pyplot import scatter
357 e5fe0e6d48a1 corrected bug computing TTC (resp. pPET) if there is no collision point (resp. crossing zone) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 345 diff changeset	283 scatter([p.x for p in points],[p.y for p in points], **kwargs)
64 c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	284
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	285 def similarOrientation(self, refDirection, cosineThreshold):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	286 'Indicates whether the cosine of the vector and refDirection is smaller than cosineThreshold'
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	287 return Point.cosine(self, refDirection) >= cosineThreshold
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	288
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	289 @staticmethod
484 6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	290 def timeToCollision(p1, p2, v1, v2, collisionThreshold):
504 a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	291 '''Computes exact time to collision with a distance threshold
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	292 The unknown of the equation is the time to reach the intersection
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	293 between the relative trajectory of one road user
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	294 and the circle of radius collisionThreshold around the other road user'''
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	295 from math import sqrt
504 a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	296 dv = v1-v2
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	297 dp = p1-p2
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	298 a = dv.norm2Squared()#(v1.x-v2.x)2 + (v1.y-v2.y)2
516 bce1fe45d1b2 corrected bugs detected by tests (because of moving functions around modules Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 515 diff changeset	299 b = 2Point.dot(dv, dp)#2 ((p1.x-p2.x) * (v1.x-v2.x) + (p1.y-p2.y) * (v1.y-v2.y))
504 a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	300 c = dp.norm2Squared() - collisionThreshold2#(p1.x-p2.x)2 + (p1.y-p2.y)2 - collisionThreshold2
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	301
504 a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	302 delta = b*2 - 4a*c
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	303 if delta >= 0:
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	304 deltaRoot = sqrt(delta)
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	305 ttc1 = (-b + deltaRoot)/(2*a)
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	306 ttc2 = (-b - deltaRoot)/(2*a)
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	307 if ttc1 >= 0 and ttc2 >= 0:
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	308 ttc = min(ttc1,ttc2)
531 f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 527 diff changeset	309 elif ttc1 >= 0:
f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 527 diff changeset	310 ttc = ttc1
f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 527 diff changeset	311 elif ttc2 >= 0:
f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 527 diff changeset	312 ttc = ttc2
f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 527 diff changeset	313 else: # ttc1 < 0 and ttc2 < 0:
f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 527 diff changeset	314 ttc = None
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	315 else:
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	316 ttc = None
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	317 return ttc
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	318
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	319
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	320 if shapelyAvailable:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	321 def pointsInPolygon(points, polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	322 '''Optimized tests of a series of points within (Shapely) polygon '''
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	323 prepared_polygon = prep(polygon)
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	324 return filter(prepared_polygon.contains, points)
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	325
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	326 # Functions for coordinate transformation
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	327 # From Paul St-Aubin's PVA tools
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	328 def subsec_spline_dist(splines):
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	329 ''' Prepare list of spline subsegments from a spline list.
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	330
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	331 Output:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	332 =======
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	333 ss_spline_d[spline #][mode][station]
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	334
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	335 where:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	336 mode=0: incremental distance
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	337 mode=1: cumulative distance
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	338 mode=2: cumulative distance with trailing distance
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	339 '''
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	340
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	341 from numpy import zeros
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	342 ss_spline_d = []
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	343 #Prepare subsegment distances
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	344 for spline in range(len(splines)):
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	345 ss_spline_d.append([[],[],[]])
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	346 ss_spline_d[spline][0] = zeros(len(splines[spline])-1) #Incremental distance
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	347 ss_spline_d[spline][1] = zeros(len(splines[spline])-1) #Cumulative distance
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	348 ss_spline_d[spline][2] = zeros(len(splines[spline])) #Cumulative distance with trailing distance
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	349 for spline_p in range(len(splines[spline])):
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	350 if spline_p > (len(splines[spline]) - 2):
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	351 break
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	352 ss_spline_d[spline][0][spline_p] = utils.pointDistanceL2(splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][(spline_p+1)][0],splines[spline][(spline_p+1)][1])
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	353 ss_spline_d[spline][1][spline_p] = sum(ss_spline_d[spline][0][0:spline_p])
568 538fb47b3007 minor modification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	354 ss_spline_d[spline][2][spline_p] = ss_spline_d[spline][1][spline_p]#sum(ss_spline_d[spline][0][0:spline_p])
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	355
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	356 ss_spline_d[spline][2][-1] = ss_spline_d[spline][2][-2] + ss_spline_d[spline][0][-1]
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	357
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	358 return ss_spline_d
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	359
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	360 def ppldb2p(qx,qy, p0x,p0y, p1x,p1y):
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	361 ''' Point-projection (Q) on line defined by 2 points (P0,P1).
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	362 http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	363 '''
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	364 if(p0x == p1x and p0y == p1y):
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	365 return None
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	366 try:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	367 #Approximate slope singularity by giving some slope roundoff; account for roundoff error
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	368 if(round(p0x, 10) == round(p1x, 10)):
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	369 p1x += 0.0000000001
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	370 if(round(p0y, 10) == round(p1y, 10)):
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	371 p1y += 0.0000000001
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	372 #make the calculation
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	373 Y = (-(qx)(p0y-p1y)-(qy(p0y-p1y)2)/(p0x-p1x)+p0x2(p0y-p1y)/(p0x-p1x)-p0xp1x(p0y-p1y)/(p0x-p1x)-p0y(p0x-p1x))/(p1x-p0x-(p0y-p1y)**2/(p0x-p1x))
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	374 X = (-Y(p1y-p0y)+qx(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x)
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	375 except ZeroDivisionError:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	376 print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:')
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	377 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y))
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	378 import pdb; pdb.set_trace()
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	379 return Point(X,Y)
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	380
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	381 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5):
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	382 ''' Snap a point p to it's nearest subsegment of it's nearest spline (from the list splines).
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	383
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	384 To force snapping to a single spline, pass the spline alone through splines (e.g. splines=[splines[splineNum]]).
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	385
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	386 Output:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	387 =======
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	388 [spline index,
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	389 subsegment leading point index,
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	390 snapped point,
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	391 subsegment distance,
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	392 spline distance,
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	393 orthogonal point offset]
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	394 '''
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	395 minOffsetY = float('inf')
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	396 #For each spline
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	397 for spline in range(len(splines)):
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	398 #For each spline point index
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	399 for spline_p in range(len(splines[spline])-1):
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	400 #Get closest point on spline
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	401 closestPoint = ppldb2p(p.x,p.y,splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][spline_p+1][0],splines[spline][spline_p+1][1])
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	402 if closestPoint == None:
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	403 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p))
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	404 return None
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	405 # check if the
fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	406 if utils.inBetween(splines[spline][spline_p][0], splines[spline][spline_p+1][0], closestPoint.x) and utils.inBetween(splines[spline][spline_p][1], splines[spline][spline_p+1][1], closestPoint.y):
fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	407 offsetY = Point.distanceNorm2(closestPoint, p)
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	408 if offsetY < minOffsetY:
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	409 minOffsetY = offsetY
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	410 snappedSpline = spline
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	411 snappedSplineLeadingPoint = spline_p
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	412 snappedPoint = Point(closestPoint.x, closestPoint.y)
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	413 #Jump loop if significantly close
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	414 if offsetY < goodEnoughSplineDistance:
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	415 break
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	416 #Get sub-segment distance
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	417 if minOffsetY != float('inf'):
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	418 subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSpline][snappedSplineLeadingPoint])
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	419 #Get cumulative alignment distance (total segment distance)
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	420 splineDistanceS = splines[snappedSpline].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	421 orthogonalSplineVector = (splines[snappedSpline][snappedSplineLeadingPoint+1]-splines[snappedSpline][snappedSplineLeadingPoint]).orthogonal()
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	422 offsetVector = p-snappedPoint
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	423 if Point.dot(orthogonalSplineVector, offsetVector) < 0:
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	424 minOffsetY = -minOffsetY
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	425 return [snappedSpline, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY]
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	426 else:
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	427 return None
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	428
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	429 def getXYfromSY(s, y, splineNum, splines, mode = 0):
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	430 ''' Find X,Y coordinate from S,Y data.
568 538fb47b3007 minor modification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	431 if mode = 0 : return Snapped X,Y
538fb47b3007 minor modification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	432 if mode !=0 : return Real X,Y
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	433 '''
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	434
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	435 #(buckle in, it gets ugly from here on out)
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	436 ss_spline_d = subsec_spline_dist(splines)
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	437
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	438 #Find subsegment
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	439 snapped_x = None
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	440 snapped_y = None
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	441 for spline_ss_index in range(len(ss_spline_d[splineNum][1])):
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	442 if(s < ss_spline_d[splineNum][1][spline_ss_index]):
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	443 ss_value = s - ss_spline_d[splineNum][1][spline_ss_index-1]
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	444 #Get normal vector and then snap
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	445 vector_l_x = (splines[splineNum][spline_ss_index][0] - splines[splineNum][spline_ss_index-1][0])
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	446 vector_l_y = (splines[splineNum][spline_ss_index][1] - splines[splineNum][spline_ss_index-1][1])
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	447 magnitude = sqrt(vector_l_x2 + vector_l_y2)
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	448 n_vector_x = vector_l_x/magnitude
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	449 n_vector_y = vector_l_y/magnitude
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	450 snapped_x = splines[splineNum][spline_ss_index-1][0] + ss_value*n_vector_x
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	451 snapped_y = splines[splineNum][spline_ss_index-1][1] + ss_value*n_vector_y
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	452
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	453 #Real values (including orthogonal projection of y))
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	454 real_x = snapped_x - y*n_vector_y
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	455 real_y = snapped_y + y*n_vector_x
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	456 break
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	457
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	458 if mode == 0 or (not snapped_x):
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	459 if(not snapped_x):
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	460 snapped_x = splines[splineNum][-1][0]
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	461 snapped_y = splines[splineNum][-1][1]
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	462 return [snapped_x,snapped_y]
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	463 else:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	464 return [real_x,real_y]
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	465
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	466
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	467 class NormAngle(object):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	468 '''Alternate encoding of a point, by its norm and orientation'''
245 bd8ab323c198 corrected issue with predictPosiont static method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 244 diff changeset	469
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	470 def __init__(self, norm, angle):
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	471 self.norm = norm
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	472 self.angle = angle
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	473
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	474 @staticmethod
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	475 def fromPoint(p):
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	476 from math import atan2
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	477 norm = p.norm2()
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	478 if norm > 0:
259 8ab76b95ee72 added code to save collision points and crossing zones in txt files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 258 diff changeset	479 angle = atan2(p.y, p.x)
339 9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	480 else:
9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	481 angle = 0.
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	482 return NormAngle(norm, angle)
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	483
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	484 def __add__(self, other):
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	485 'a norm cannot become negative'
255 13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	486 return NormAngle(max(self.norm+other.norm, 0), self.angle+other.angle)
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	487
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	488 def getPoint(self):
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	489 from math import cos, sin
255 13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	490 return Point(self.normcos(self.angle), self.normsin(self.angle))
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	491
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	492
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	493 def predictPositionNoLimit(nTimeSteps, initialPosition, initialVelocity, initialAcceleration = Point(0,0)):
245 bd8ab323c198 corrected issue with predictPosiont static method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 244 diff changeset	494 '''Predicts the position in nTimeSteps at constant speed/acceleration'''
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	495 return initialVelocity + initialAcceleration.multiply(nTimeSteps),initialPosition+initialVelocity.multiply(nTimeSteps) + initialAcceleration.multiply(nTimeSteps*20.5)
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	496
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	497 def predictPosition(position, speedOrientation, control, maxSpeed = None):
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	498 '''Predicts the position (moving.Point) at the next time step with given control input (deltaSpeed, deltaTheta)
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	499 speedOrientation is the other encoding of velocity, (speed, orientation)
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	500 speedOrientation and control are NormAngle'''
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	501 predictedSpeedTheta = speedOrientation+control
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	502 if maxSpeed:
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	503 predictedSpeedTheta.norm = min(predictedSpeedTheta.norm, maxSpeed)
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	504 predictedPosition = position+predictedSpeedTheta.getPoint()
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	505 return predictedPosition, predictedSpeedTheta
245 bd8ab323c198 corrected issue with predictPosiont static method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 244 diff changeset	506
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	507
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	508 class FlowVector(object):
184 d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	509 '''Class to represent 4-D flow vectors,
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	510 ie a position and a velocity'''
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	511 def __init__(self, position, velocity):
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	512 'position and velocity should be Point instances'
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	513 self.position = position
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	514 self.velocity = velocity
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	515
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	516 def __add__(self, other):
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	517 return FlowVector(self.position+other.position, self.velocity+other.velocity)
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	518
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	519 def multiply(self, alpha):
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	520 return FlowVector(self.position.multiply(alpha), self.velocity.multiply(alpha))
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	521
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	522 def plot(self, options = '', **kwargs):
184 d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	523 from matplotlib.pylab import plot
211 ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	524 plot([self.position.x, self.position.x+self.velocity.x], [self.position.y, self.position.y+self.velocity.y], options, **kwargs)
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	525 self.position.plot(options+'x', **kwargs)
184 d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	526
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	527 @staticmethod
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	528 def similar(f1, f2, maxDistance2, maxDeltavelocity2):
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	529 return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	530
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	531 def intersection(p1, p2, p3, p4):
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	532 ''' Intersection point (x,y) of lines formed by the vectors p1-p2 and p3-p4
571 a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	533 http://paulbourke.net/geometry/pointlineplane/'''
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	534 dp12 = p2-p1
571 a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	535 dp34 = p4-p3
a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	536 #det = (p4.y-p3.y)(p2.x-p1.x)-(p4.x-p3.x)(p2.y-p1.y)
a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	537 det = dp34.ydp12.x-dp34.xdp12.y
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	538 if det == 0:
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	539 return None
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	540 else:
571 a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	541 ua = (dp34.x(p1.y-p3.y)-dp34.y(p1.x-p3.x))/det
a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	542 return p1+dp12.multiply(ua)
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	543
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	544 # def intersection(p1, p2, dp1, dp2):
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	545 # '''Returns the intersection point between the two lines
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	546 # defined by the respective vectors (dp) and origin points (p)'''
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	547 # from numpy import matrix
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	548 # from numpy.linalg import linalg
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	549 # A = matrix([[dp1.y, -dp1.x],
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	550 # [dp2.y, -dp2.x]])
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	551 # B = matrix([[dp1.yp1.x-dp1.xp1.y],
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	552 # [dp2.yp2.x-dp2.xp2.y]])
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	553
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	554 # if linalg.det(A) == 0:
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	555 # return None
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	556 # else:
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	557 # intersection = linalg.solve(A,B)
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	558 # return Point(intersection[0,0], intersection[1,0])
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	559
152 74b1fc68d4df re-organized code to avoid cyclic python module dependencies Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 148 diff changeset	560 def segmentIntersection(p1, p2, p3, p4):
74b1fc68d4df re-organized code to avoid cyclic python module dependencies Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 148 diff changeset	561 '''Returns the intersecting point of the segments [p1, p2] and [p3, p4], None otherwise'''
74b1fc68d4df re-organized code to avoid cyclic python module dependencies Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 148 diff changeset	562
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	563 if (Interval.intersection(Interval(p1.x,p2.x,True), Interval(p3.x,p4.x,True)).empty()) or (Interval.intersection(Interval(p1.y,p2.y,True), Interval(p3.y,p4.y,True)).empty()):
152 74b1fc68d4df re-organized code to avoid cyclic python module dependencies Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 148 diff changeset	564 return None
74b1fc68d4df re-organized code to avoid cyclic python module dependencies Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 148 diff changeset	565 else:
571 a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	566 inter = intersection(p1, p2, p3, p4)
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	567 if (inter != None
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	568 and utils.inBetween(p1.x, p2.x, inter.x)
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	569 and utils.inBetween(p3.x, p4.x, inter.x)
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	570 and utils.inBetween(p1.y, p2.y, inter.y)
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	571 and utils.inBetween(p3.y, p4.y, inter.y)):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	572 return inter
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	573 else:
258 d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	574 return None
d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	575
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	576 class Trajectory(object):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	577 '''Class for trajectories: temporal sequence of positions
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	578
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	579 The class is iterable'''
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	580
79 5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	581 def __init__(self, positions=None):
331 40790d93200e corrected a bug for Trajectory initialization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 329 diff changeset	582 if positions != None:
113 606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	583 self.positions = positions
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	584 else:
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	585 self.positions = [[],[]]
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	586
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	587 @staticmethod
555 f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	588 def generate(p, v, nPoints):
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	589 t = Trajectory()
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	590 p0 = Point(p.x, p.y)
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	591 t.addPosition(p0)
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	592 for i in xrange(nPoints-1):
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	593 p0 += v
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	594 t.addPosition(p0)
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	595 return t, Trajectory([[v.x]nPoints, [v.y]nPoints])
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	596
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	597 @staticmethod
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	598 def load(line1, line2):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	599 return Trajectory([[float(n) for n in line1.split(' ')],
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	600 [float(n) for n in line2.split(' ')]])
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	601
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	602 @staticmethod
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	603 def fromPointList(points):
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	604 t = Trajectory()
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	605 if isinstance(points[0], list) or isinstance(points[0], tuple):
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	606 for p in points:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	607 t.addPositionXY(p[0],p[1])
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	608 else:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	609 for p in points:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	610 t.addPosition(p)
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	611 return t
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	612
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	613 def __len__(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	614 return len(self.positions[0])
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	615
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	616 def length(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	617 return self.__len__()
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	618
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	619 def __getitem__(self, i):
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	620 if isinstance(i, int):
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	621 return Point(self.positions[0][i], self.positions[1][i])
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	622 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	623 raise TypeError, "Invalid argument type."
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	624 #elif isinstance( key, slice ):
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	625
16 9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	626 def __str__(self):
39 e47168f6b694 corrected printing bug Nicolas Saunier <nico@confins.net> parents: 38 diff changeset	627 return ' '.join([self.__getitem__(i).__str__() for i in xrange(self.length())])
16 9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	628
69 cc192d0450b3 added full support for two implementations of indicators, with tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 68 diff changeset	629 def __repr__(self):
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	630 return self.__str__()
23 5f2921ad4f7e made Trajectory indexable and timeinterval iterable Nicolas Saunier <nico@confins.net> parents: 22 diff changeset	631
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	632
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	633 def __iter__(self):
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	634 self.iterInstantNum = 0
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	635 return self
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	636
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	637 def next(self):
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	638 if self.iterInstantNum >= self.length():
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	639 raise StopIteration
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	640 else:
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	641 self.iterInstantNum += 1
23 5f2921ad4f7e made Trajectory indexable and timeinterval iterable Nicolas Saunier <nico@confins.net> parents: 22 diff changeset	642 return self[self.iterInstantNum-1]
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	643
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	644 def setPositionXY(self, i, x, y):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	645 if i < self.__len__():
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	646 self.positions[0][i] = x
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	647 self.positions[1][i] = y
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	648
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	649 def setPosition(self, i, p):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	650 self.setPositionXY(i, p.x, p.y)
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	651
71 45e958ccd9bd added new addPosition method to Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 69 diff changeset	652 def addPositionXY(self, x, y):
113 606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	653 self.positions[0].append(x)
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	654 self.positions[1].append(y)
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	655
71 45e958ccd9bd added new addPosition method to Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 69 diff changeset	656 def addPosition(self, p):
79 5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	657 self.addPositionXY(p.x, p.y)
182 d3f6de6c3918 added drawing functialities, in particular on aerial image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 152 diff changeset	658
343 74e437ab5f11 first version of indicator loading code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 339 diff changeset	659 def duplicateLastPosition(self):
74e437ab5f11 first version of indicator loading code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 339 diff changeset	660 self.positions[0].append(self.positions[0][-1])
74e437ab5f11 first version of indicator loading code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 339 diff changeset	661 self.positions[1].append(self.positions[1][-1])
74e437ab5f11 first version of indicator loading code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 339 diff changeset	662
182 d3f6de6c3918 added drawing functialities, in particular on aerial image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 152 diff changeset	663 @staticmethod
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	664 def _plot(positions, options = '', withOrigin = False, lastCoordinate = None, timeStep = 1, **kwargs):
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	665 from matplotlib.pylab import plot
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	666 if lastCoordinate == None:
270 05c9b0cb8202 updates for drawing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 268 diff changeset	667 plot(positions[0][::timeStep], positions[1][::timeStep], options, **kwargs)
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	668 elif 0 <= lastCoordinate <= len(positions[0]):
270 05c9b0cb8202 updates for drawing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 268 diff changeset	669 plot(positions[0][:lastCoordinate:timeStep], positions[1][:lastCoordinate:timeStep], options, **kwargs)
182 d3f6de6c3918 added drawing functialities, in particular on aerial image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 152 diff changeset	670 if withOrigin:
211 ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	671 plot([positions[0][0]], [positions[1][0]], 'ro', **kwargs)
182 d3f6de6c3918 added drawing functialities, in particular on aerial image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 152 diff changeset	672
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	673 def project(self, homography):
318 82a8fba99bec minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 317 diff changeset	674 return Trajectory(cvutils.projectTrajectory(homography, self.positions))
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	675
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	676 def plot(self, options = '', withOrigin = False, timeStep = 1, **kwargs):
727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	677 Trajectory._plot(self.positions, options, withOrigin, None, timeStep, **kwargs)
182 d3f6de6c3918 added drawing functialities, in particular on aerial image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 152 diff changeset	678
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	679 def plotAt(self, lastCoordinate, options = '', withOrigin = False, timeStep = 1, **kwargs):
727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	680 Trajectory._plot(self.positions, options, withOrigin, lastCoordinate, timeStep, **kwargs)
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	681
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	682 def plotOnWorldImage(self, nPixelsPerUnitDistance, imageHeight, options = '', withOrigin = False, timeStep = 1, **kwargs):
182 d3f6de6c3918 added drawing functialities, in particular on aerial image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 152 diff changeset	683 from matplotlib.pylab import plot
d3f6de6c3918 added drawing functialities, in particular on aerial image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 152 diff changeset	684 imgPositions = [[x*nPixelsPerUnitDistance for x in self.positions[0]],
d3f6de6c3918 added drawing functialities, in particular on aerial image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 152 diff changeset	685 [-x*nPixelsPerUnitDistance+imageHeight for x in self.positions[1]]]
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	686 Trajectory._plot(imgPositions, options, withOrigin, timeStep, **kwargs)
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	687
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	688 def getXCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	689 return self.positions[0]
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	690
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	691 def getYCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	692 return self.positions[1]
92 a5ef9e40688e makes use of matplotlib function to test if point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 91 diff changeset	693
a5ef9e40688e makes use of matplotlib function to test if point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 91 diff changeset	694 def asArray(self):
a5ef9e40688e makes use of matplotlib function to test if point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 91 diff changeset	695 from numpy.core.multiarray import array
a5ef9e40688e makes use of matplotlib function to test if point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 91 diff changeset	696 return array(self.positions)
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	697
14 e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	698 def xBounds(self):
e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	699 # look for function that does min and max in one pass
258 d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	700 return Interval(min(self.getXCoordinates()), max(self.getXCoordinates()))
14 e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	701
e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	702 def yBounds(self):
e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	703 # look for function that does min and max in one pass
258 d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	704 return Interval(min(self.getYCoordinates()), max(self.getYCoordinates()))
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	705
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	706 def add(self, traj2):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	707 '''Returns a new trajectory of the same length'''
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	708 if self.length() != traj2.length():
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	709 print 'Trajectories of different lengths'
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	710 return None
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	711 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	712 return Trajectory([[a+b for a,b in zip(self.getXCoordinates(),traj2.getXCoordinates())],
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	713 [a+b for a,b in zip(self.getYCoordinates(),traj2.getYCoordinates())]])
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	714
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	715 def subtract(self, traj2):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	716 '''Returns a new trajectory of the same length'''
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	717 if self.length() != traj2.length():
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	718 print 'Trajectories of different lengths'
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	719 return None
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	720 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	721 return Trajectory([[a-b for a,b in zip(self.getXCoordinates(),traj2.getXCoordinates())],
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	722 [a-b for a,b in zip(self.getYCoordinates(),traj2.getYCoordinates())]])
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	723
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	724 def differentiate(self, doubleLastPosition = False):
339 9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	725 diff = Trajectory()
9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	726 for i in xrange(1, self.length()):
9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	727 diff.addPosition(self[i]-self[i-1])
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	728 if doubleLastPosition:
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	729 diff.addPosition(diff[-1])
339 9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	730 return diff
9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	731
16 9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	732 def norm(self):
9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	733 '''Returns the list of the norms at each instant'''
9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	734 # def add(x, y): return x+y
9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	735 # sq = map(add, [xx for x in self.positions[0]], [yy for y in self.positions[1]])
9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	736 # return sqrt(sq)
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	737 from numpy import hypot
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	738 return hypot(self.positions[0], self.positions[1])
14 e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	739
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	740 # def cumulatedDisplacement(self):
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	741 # 'Returns the sum of the distances between each successive point'
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	742 # displacement = 0
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	743 # for i in xrange(self.length()-1):
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	744 # displacement += Point.distanceNorm2(self.__getitem__(i),self.__getitem__(i+1))
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	745 # return displacement
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	746
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	747 def computeCumulativeDistances(self):
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	748 '''Computes the distance from each point to the next and the cumulative distance up to the point
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	749 Can be accessed through getDistance(idx) and getCumulativeDistance(idx)'''
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	750 self.distances = []
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	751 self.cumulativeDistances = [0.]
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	752 p1 = self[0]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	753 cumulativeDistance = 0.
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	754 for i in xrange(self.length()-1):
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	755 p2 = self[i+1]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	756 self.distances.append(Point.distanceNorm2(p1,p2))
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	757 cumulativeDistance += self.distances[-1]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	758 self.cumulativeDistances.append(cumulativeDistance)
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	759 p1 = p2
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	760
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	761 def getDistance(self,i):
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	762 '''Return the distance between points i and i+1'''
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	763 if i < self.length()-1:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	764 return self.distances[i]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	765 else:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	766 print('Index {} beyond trajectory length {}-1'.format(i, self.length()))
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	767
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	768 def getCumulativeDistance(self, i):
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	769 '''Return the cumulative distance between the beginning and point i'''
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	770 if i < self.length():
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	771 return self.cumulativeDistances[i]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	772 else:
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	773 print('Index {} beyond trajectory length {}'.format(i, self.length()))
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	774
381 387cc0142211 script to replay event annotations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 379 diff changeset	775 def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5):
387cc0142211 script to replay event annotations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 379 diff changeset	776 '''Indicates whether the minProportion (<=1.) (eg half) of the trajectory elements (vectors for velocity)
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	777 have a cosine with refDirection is smaller than cosineThreshold'''
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	778 count = 0
383 0ce2210790b1 fixed stupid naming bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 381 diff changeset	779 lengthThreshold = float(self.length())*minProportion
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	780 for p in self:
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	781 if p.similarOrientation(refDirection, cosineThreshold):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	782 count += 1
381 387cc0142211 script to replay event annotations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 379 diff changeset	783 if count > lengthThreshold:
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	784 return True
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	785 return False
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	786
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	787 def wiggliness(self):
1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	788 return self.cumulatedDisplacement()/float(Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1)))
1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	789
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	790 def getIntersections(self, p1, p2):
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	791 '''Returns a list of the indices at which the trajectory
91 daa05fae1a70 modified the type of the result of interval lengths to float, added comments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 90 diff changeset	792 intersects with the segment of extremities p1 and p2
daa05fae1a70 modified the type of the result of interval lengths to float, added comments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 90 diff changeset	793 the list is empty if there is no crossing'''
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	794 indices = []
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	795
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	796 for i in xrange(self.length()-1):
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	797 q1=self.__getitem__(i)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	798 q2=self.__getitem__(i+1)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	799 p = utils.segmentIntersection(q1, q2, p1, p2)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	800 if p:
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	801 if q1.x != q2.x:
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	802 ratio = (p.x-q1.x)/(q2.x-q1.x)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	803 elif q1.y != q2.y:
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	804 ratio = (p.y-q1.y)/(q2.y-q1.y)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	805 else:
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	806 ratio = 0
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	807 indices.append(i+ratio)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	808 return indices
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	809
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	810 def getTrajectoryInInterval(self, inter):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	811 if inter.first >=0 and inter.last<= self.length():
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	812 return Trajectory([self.positions[0][inter.first:inter.last],
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	813 self.positions[1][inter.first:inter.last]])
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	814 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	815 return None
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	816
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	817 def getTrajectoryInPolygonNoShapely(self, polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	818 '''Returns the trajectory built with the set of points inside the polygon
92 a5ef9e40688e makes use of matplotlib function to test if point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 91 diff changeset	819 (array of Nx2 coordinates of the polygon vertices)'''
113 606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	820 traj = Trajectory()
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	821 for p in self:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	822 if p.inPolygonNoShapely(polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	823 traj.addPosition(p)
113 606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	824 return traj
79 5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	825
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	826 if shapelyAvailable:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	827 def getTrajectoryInPolygon(self, polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	828 '''Returns the trajectory built with the set of points inside the (shapely) polygon'''
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	829 traj = Trajectory()
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	830 points = [p.asShapely() for p in self]
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	831 for p in pointsInPolygon(points, polygon):
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	832 traj.addPositionXY(p.x, p.y)
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	833 return traj
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	834
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	835 @staticmethod
369 027e254f0b53 lcss subclass for indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 368 diff changeset	836 def lcss(t1, t2, lcss):
027e254f0b53 lcss subclass for indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 368 diff changeset	837 return lcss.compute(t1, t2)
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	838
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	839 class CurvilinearTrajectory(Trajectory):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	840 '''Sub class of trajectory for trajectories with curvilinear coordinates and lane assignements
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	841 longitudinal coordinate is stored as first coordinate (exterior name S)
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	842 lateral coordiante is stored as second coordinate'''
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	843
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	844 def __init__(self, S = None, Y = None, lanes = None):
543 cb213269d330 defensive code for curvilinear trajectory initialization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 542 diff changeset	845 if S == None or Y == None or len(S) != len(Y):
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	846 self.positions = [[],[]]
552 ca6bded754ac corrected more bugs from merging (caught in regression tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 551 diff changeset	847 if S != None and Y != None and len(S) != len(Y):
543 cb213269d330 defensive code for curvilinear trajectory initialization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 542 diff changeset	848 print("S and Y coordinates of different lengths\nInitializing to empty lists")
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	849 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	850 self.positions = [S,Y]
543 cb213269d330 defensive code for curvilinear trajectory initialization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 542 diff changeset	851 if lanes == None or len(lanes) != self.length():
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	852 self.lanes = []
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	853 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	854 self.lanes = lanes
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	855
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	856 def __getitem__(self,i):
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	857 if isinstance(i, int):
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	858 return [self.positions[0][i], self.positions[1][i], self.lanes[i]]
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	859 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	860 raise TypeError, "Invalid argument type."
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	861 #elif isinstance( key, slice ):
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	862
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	863 def getSCoordinates(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	864 return self.getXCoordinates()
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	865
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	866 def getLanes(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	867 return self.lanes
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	868
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	869 def addPositionSYL(self, s, y, lane):
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	870 self.addPositionXY(s,y)
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	871 self.lanes.append(lane)
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	872
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	873 def addPosition(self, p):
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	874 'Adds position in the point format for curvilinear of list with 3 values'
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	875 self.addPositionSYL(p[0], p[1], p[2])
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	876
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	877 def setPosition(self, i, s, y, lane):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	878 self.setPositionXY(i, s, y)
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	879 if i < self.__len__():
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	880 self.lanes[i] = lane
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	881
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	882 def differentiate(self, doubleLastPosition = False):
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	883 diff = CurvilinearTrajectory()
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	884 p1 = self[0]
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	885 for i in xrange(1, self.length()):
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	886 p2 = self[i]
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	887 diff.addPositionSYL(p2[0]-p1[0], p2[1]-p1[1], p1[2])
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	888 p1=p2
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	889 if doubleLastPosition:
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	890 diff.addPosition(diff[-1])
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	891 return diff
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	892
451 cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	893 def getIntersections(self, S1, lane = None):
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	894 '''Returns a list of the indices at which the trajectory
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	895 goes past the curvilinear coordinate S1
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	896 (in provided lane if lane != None)
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	897 the list is empty if there is no crossing'''
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	898 indices = []
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	899 for i in xrange(self.length()-1):
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	900 q1=self.__getitem__(i)
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	901 q2=self.__getitem__(i+1)
527 37830a831818 loading from VISSIM trajectory data works Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 526 diff changeset	902 if q1[0] <= S1 < q2[0] and (lane == None or (self.lanes[i] == lane and self.lanes[i+1] == lane)):
451 cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	903 indices.append(i+(S1-q1[0])/(q2[0]-q1[0]))
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	904 return indices
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	905
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	906 ##################
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	907 # Moving Objects
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	908 ##################
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	909
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	910 userTypeNames = ['unknown',
a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	911 'car',
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	912 'pedestrian',
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	913 'motorcycle',
a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	914 'bicycle',
185 c06379f25ab8 utilities for user types Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 184 diff changeset	915 'bus',
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	916 'truck']
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	917
185 c06379f25ab8 utilities for user types Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 184 diff changeset	918 userType2Num = utils.inverseEnumeration(userTypeNames)
c06379f25ab8 utilities for user types Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 184 diff changeset	919
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	920 class MovingObject(STObject):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	921 '''Class for moving objects: a spatio-temporal object
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	922 with a trajectory and a geometry (constant volume over time) and a usertype (e.g. road user) coded as a number (see
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	923 '''
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	924
411 31604ef1cad4 added hog functions and the display of road user types if known Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 383 diff changeset	925 def __init__(self, num = None, timeInterval = None, positions = None, velocities = None, geometry = None, userType = userType2Num['unknown']):
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	926 super(MovingObject, self).__init__(num, timeInterval)
16 9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	927 self.positions = positions
345 fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	928 self.velocities = velocities
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	929 self.geometry = geometry
54 c354d41ef7cd corrected code for usertype in movingobject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 49 diff changeset	930 self.userType = userType
517 47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	931 self.features = []
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	932 # compute bounding polygon from trajectory
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	933
555 f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	934 @staticmethod
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	935 def generate(p, v, timeInterval):
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	936 positions, velocities = Trajectory.generate(p, v, int(timeInterval.length()))
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	937 return MovingObject(timeInterval = timeInterval, positions = positions, velocities = velocities)
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	938
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	939 def getObjectInTimeInterval(self, inter):
54 c354d41ef7cd corrected code for usertype in movingobject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 49 diff changeset	940 '''Returns a new object extracted from self,
c354d41ef7cd corrected code for usertype in movingobject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 49 diff changeset	941 restricted to time interval inter'''
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	942 intersection = TimeInterval.intersection(inter, self.getTimeInterval())
97 b3a1c26e2f22 corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 96 diff changeset	943 if not intersection.empty():
b3a1c26e2f22 corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 96 diff changeset	944 trajectoryInterval = TimeInterval(intersection.first-self.getFirstInstant(), intersection.last-self.getFirstInstant())
b3a1c26e2f22 corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 96 diff changeset	945 obj = MovingObject(self.num, intersection, self.positions.getTrajectoryInInterval(trajectoryInterval), self.geometry, self.userType)
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	946 if self.velocities:
97 b3a1c26e2f22 corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 96 diff changeset	947 obj.velocities = self.velocities.getTrajectoryInInterval(trajectoryInterval)
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	948 return obj
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	949 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	950 print 'The object does not exist at '+str(inter)
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	951 return None
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	952
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	953 def length(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	954 return self.timeInterval.length()
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	955
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	956 def getPositions(self):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	957 return self.positions
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	958
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	959 def getVelocities(self):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	960 return self.velocities
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	961
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	962 def getUserType(self):
a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	963 return self.userType
a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	964
335 3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	965 def getCurvilinearPositions(self):
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	966 if hasattr(self, 'curvilinearPositions'):
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	967 return self.curvilinearPositions
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	968 else:
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	969 return None
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	970
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	971 def setUserType(self, userType):
a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	972 self.userType = userType
a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	973
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 259 diff changeset	974 def setFeatures(self, features):
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 259 diff changeset	975 self.features = [features[i] for i in self.featureNumbers]
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 259 diff changeset	976
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	977 def getSpeeds(self):
1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	978 return self.getVelocities().norm()
1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	979
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	980 def getPositionAt(self, i):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	981 return self.positions[i]
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	982
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	983 def getVelocityAt(self, i):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	984 return self.velocities[i]
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	985
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	986 def getPositionAtInstant(self, i):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	987 return self.positions[i-self.getFirstInstant()]
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	988
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	989 def getVelocityAtInstant(self, i):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	990 return self.velocities[i-self.getFirstInstant()]
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	991
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	992 def getXCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	993 return self.positions.getXCoordinates()
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	994
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	995 def getYCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	996 return self.positions.getYCoordinates()
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	997
517 47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	998 def plot(self, options = '', withOrigin = False, timeStep = 1, withFeatures = False, **kwargs):
47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	999 if withFeatures:
47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1000 for f in self.features:
47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1001 f.positions.plot('r', True, timeStep, **kwargs)
47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1002 self.positions.plot('bx-', True, timeStep, **kwargs)
47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1003 else:
47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1004 self.positions.plot(options, withOrigin, timeStep, **kwargs)
182 d3f6de6c3918 added drawing functialities, in particular on aerial image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 152 diff changeset	1005
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	1006 def plotOnWorldImage(self, nPixelsPerUnitDistance, imageHeight, options = '', withOrigin = False, timeStep = 1, **kwargs):
727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	1007 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, imageHeight, options, withOrigin, timeStep, **kwargs)
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	1008
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	1009 def play(self, videoFilename, homography = None):
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	1010 cvutils.displayTrajectories(videoFilename, [self], homography, self.getFirstInstant(), self.getLastInstant())
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	1011
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1012 def speedDiagnostics(self, framerate = 1., display = False):
377 2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1013 from numpy import std
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1014 from scipy.stats import scoreatpercentile
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1015 speeds = framerate*self.getSpeeds()
377 2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1016 coef = utils.linearRegression(range(len(speeds)), speeds)
379 f1a1923ddff9 minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 378 diff changeset	1017 print('min/5th perc speed: {} / {}\nspeed diff: {}\nspeed stdev: {}\nregression: {}'.format(min(speeds), scoreatpercentile(speeds, 5), speeds[-2]-speeds[1], std(speeds), coef[0]))
377 2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1018 if display:
379 f1a1923ddff9 minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 378 diff changeset	1019 from matplotlib.pyplot import figure, plot, axis
377 2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1020 figure(1)
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	1021 self.plot()
379 f1a1923ddff9 minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 378 diff changeset	1022 axis('equal')
377 2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1023 figure(2)
2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1024 plot(list(self.getTimeInterval()), speeds)
2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1025
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1026 @staticmethod
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1027 def distances(obj1, obj2, instant1, _instant2 = None):
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1028 from scipy.spatial.distance import cdist
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1029 if _instant2 == None:
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1030 instant2 = instant1
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1031 else:
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1032 instant2 = _instant2
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1033 positions1 = [f.getPositionAtInstant(instant1).astuple() for f in obj1.features if f.existsAtInstant(instant1)]
6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1034 positions2 = [f.getPositionAtInstant(instant2).astuple() for f in obj2.features if f.existsAtInstant(instant2)]
6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1035 return cdist(positions1, positions2, metric = 'euclidean')
6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1036
6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1037 @staticmethod
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1038 def minDistance(obj1, obj2, instant1, instant2 = None):
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1039 return MovingObject.distances(obj1, obj2, instant1, instant2).min()
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1040
6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1041 @staticmethod
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1042 def maxDistance(obj1, obj2, instant, instant2 = None):
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1043 return MovingObject.distances(obj1, obj2, instant1, instant2).max()
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1044
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1045 def maxSize(self):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1046 '''Returns the max distance between features
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1047 at instant there are the most features'''
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1048 if hasattr(self, 'features'):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1049 nFeatures = -1
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1050 tMaxFeatures = 0
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1051 for t in self.getTimeInterval():
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1052 n = len([f for f in self.features if f.existsAtInstant(t)])
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1053 if n > nFeatures:
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1054 nFeatures = n
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1055 tMaxFeatures = t
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1056 return MovingObject.maxDistance(self, self, tMaxFeatures)
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1057 else:
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1058 print('Load features to compute a maximum size')
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1059 return None
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1060
550 5668af2ff515 minor naming Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 549 diff changeset	1061 def setRoutes(self, startRouteID, endRouteID):
5668af2ff515 minor naming Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 549 diff changeset	1062 self.startRouteID = startRouteID
5668af2ff515 minor naming Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 549 diff changeset	1063 self.endRouteID = endRouteID
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1064
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1065 def getInstantsCrossingLane(self, p1, p2):
55 88d5ee5ac164 updated comments and added shell for interaction between road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 54 diff changeset	1066 '''Returns the instant(s)
88d5ee5ac164 updated comments and added shell for interaction between road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 54 diff changeset	1067 at which the object passes from one side of the segment to the other
88d5ee5ac164 updated comments and added shell for interaction between road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 54 diff changeset	1068 empty list if there is no crossing'''
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1069 indices = self.positions.getIntersections(p1, p2)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1070 return [t+self.getFirstInstant() for t in indices]
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	1071
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	1072 def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)):
243 e0988a8ace0c started adapting and moving to other modules Mohamed's work Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 223 diff changeset	1073 '''Predicts the position of object at instant+deltaT,
e0988a8ace0c started adapting and moving to other modules Mohamed's work Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 223 diff changeset	1074 at constant speed'''
255 13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	1075 return predictPositionNoLimit(nTimeSteps, self.getPositionAtInstant(instant), self.getVelocityAtInstant(instant), externalAcceleration)
243 e0988a8ace0c started adapting and moving to other modules Mohamed's work Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 223 diff changeset	1076
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1077 def projectCurvilinear(self, alignments, ln_mv_av_win=3):
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1078 ''' Add, for every object position, the class 'moving.CurvilinearTrajectory()'
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1079 (curvilinearPositions instance) which holds information about the
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1080 curvilinear coordinates using alignment metadata.
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1081 From Paul St-Aubin's PVA tools
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1082 ======
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1083
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1084 Input:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1085 ======
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1086 alignments = a list of alignments, where each alignment is a list of
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1087 points, where each point is a list of coordinates, e.g.
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1088 alignments[alpha][1][x] is coordinate x for point number
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1089 1 of the spline that represents alignment alpha.
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1090 alignments can also be a compatible object that mimics a
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1091 3-dimensional list using the __getitem__() method as is
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1092 the case in the PVAT specification.
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1093 ln_mv_av_win = moving average window (in points) in which to smooth
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1094 lane changes. As per tools_math.cat_mvgavg(), this term
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1095 is a search radius around the center of the window.
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1096
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1097 '''
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1098
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1099 self.curvilinearPositions = CurvilinearTrajectory()
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1100
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1101 #For each point
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1102 for p in self.getPositions():#xrange(int(self.length())):
fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1103 result = getSYfromXY(p, alignments)
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1104
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1105 # Error handling
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1106 if(result == None):
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1107 print('Warning: trajectory {} at point {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), p))
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1108 else:
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1109 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1110 self.curvilinearPositions.addPositionSYL(S, Y, align)
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1111
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1112 ## Go back through points and correct lane
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1113 #Run through objects looking for outlier point
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1114 smoothed_lanes = utils.cat_mvgavg(self.curvilinearPositions.getLanes(),ln_mv_av_win)
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1115 ## Recalculate projected point to new lane
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1116 lanes = self.curvilinearPositions.getLanes()
fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1117 if(lanes != smoothed_lanes):
fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1118 for i in range(int(self.length())):
fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1119 if(lanes[i] != smoothed_lanes[i]):
fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1120 result = getSYfromXY(self.getPositionAt(i),[alignments[smoothed_lanes[i]]])
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1121
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1122 # Error handling
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1123 if(result == None):
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1124 ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment.
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1125 print(' Warning: trajectory {} at point {} {} has alignment errors during trajectory smoothing and will not be corrected.'.format(self.getNum(), i, self.getPositionAt(i)))
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1126 else:
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1127 [align, alignPoint, snapped_x, snapped_y, subsegmentDistance, S, Y] = result
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1128 self.curvilinearPositions.setPosition(i, S, Y, align)
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1129
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1130 #Resize objects
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1131 # if(len(dropped_traj) > 0):
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1132 # objects = filter(None, objects)
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1133 # if(verbose >= 2): print(' Filtering report: Trajectories dropped: '+str(len(dropped_traj)))
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1134 #if(verbose >= 2): print(' Filtering report: Lane observation corrections per object: '+str(lane_readjustments/original_object_length))
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1135
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1136 #if(reset_objects and len(objects) > 0): return objects[0], dropped_traj
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1137 #else: return objects, dropped_traj
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1138
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1139
562 259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1140 def computeSmoothTrajectory(self, minCommonIntervalLength):
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1141 '''Computes the trajectory as the mean of all features
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1142 if a feature exists, its position is
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1143
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1144 Warning work in progress
562 259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1145 TODO? not use the first/last 1-.. positions'''
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1146 from numpy import array, median
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1147 nFeatures = len(self.features)
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1148 if nFeatures == 0:
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1149 print('Empty object features\nCannot compute smooth trajectory')
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1150 else:
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1151 # compute the relative position vectors
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1152 relativePositions = {} # relativePositions[(i,j)] is the position of j relative to i
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1153 for i in xrange(nFeatures):
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1154 for j in xrange(i):
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1155 fi = self.features[i]
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1156 fj = self.features[j]
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1157 inter = fi.commonTimeInterval(fj)
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1158 if inter.length() >= minCommonIntervalLength:
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1159 xi = array(fi.getXCoordinates()[inter.first-fi.getFirstInstant():int(fi.length())-(fi.getLastInstant()-inter.last)])
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1160 yi = array(fi.getYCoordinates()[inter.first-fi.getFirstInstant():int(fi.length())-(fi.getLastInstant()-inter.last)])
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1161 xj = array(fj.getXCoordinates()[inter.first-fj.getFirstInstant():int(fj.length())-(fj.getLastInstant()-inter.last)])
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1162 yj = array(fj.getYCoordinates()[inter.first-fj.getFirstInstant():int(fj.length())-(fj.getLastInstant()-inter.last)])
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1163 relativePositions[(i,j)] = Point(median(xj-xi), median(yj-yi))
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1164 relativePositions[(j,i)] = -relativePositions[(i,j)]
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1165
522 ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1166 ###
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1167 # User Type Classification
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1168 ###
524 1dced8932b08 corrected bugs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 523 diff changeset	1169 def classifyUserTypeSpeedMotorized(self, threshold, aggregationFunc = median, ignoreNInstantsAtEnds = 0):
345 fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1170 '''Classifies slow and fast road users
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1171 slow: non-motorized -> pedestrians
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1172 fast: motorized -> cars'''
501 c81cbd6953fb update to classify speed to remove data at both ends Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 484 diff changeset	1173 if ignoreNInstantsAtEnds > 0:
522 ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1174 speeds = self.getSpeeds()[ignoreNInstantsAtEnds:-ignoreNInstantsAtEnds]
501 c81cbd6953fb update to classify speed to remove data at both ends Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 484 diff changeset	1175 else:
522 ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1176 speeds = self.getSpeeds()
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1177 if aggregationFunc(speeds) >= threshold:
345 fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1178 self.setUserType(userType2Num['car'])
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1179 else:
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1180 self.setUserType(userType2Num['pedestrian'])
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1181
524 1dced8932b08 corrected bugs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 523 diff changeset	1182 def classifyUserTypeSpeed(self, speedProbabilities, aggregationFunc = median):
522 ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1183 '''Classifies road user per road user type
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1184 speedProbabilities are functions return P(speed\|class)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1185 in a dictionary indexed by user type names
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1186 Returns probabilities for each class
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1187
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1188 for simple threshold classification, simply pass non-overlapping indicator functions (membership)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1189 e.g. def indic(x):
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1190 if abs(x-mu) < sigma:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1191 return 1
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1192 else:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1193 return x'''
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1194 if not hasattr(self, aggregatedSpeed):
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1195 self.aggregatedSpeed = aggregationFunc(self.getSpeeds())
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1196 userTypeProbabilities = {userType2Num[userTypename]: speedProbabilities[userTypename](self.aggregatedSpeed) for userTypename in speedProbabilities}
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1197 self.setUserType(utils.argmaxDict(userTypeProbabilities))
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1198 return userTypeProbabilities
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1199
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1200 def initClassifyUserTypeHoGSVM(self, aggregationFunc = median):
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1201 '''Initializes the data structures for classification
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1202
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1203 TODO? compute speed for longest feature?
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1204 Skip beginning and end of feature for speed? Offer options instead of median'''
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1205 self.aggregatedSpeed = aggregationFunc(self.getSpeeds())
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1206 self.userTypes = {}
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1207
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1208 def classifyUserTypeHoGSVMAtInstant(self, img, pedBikeCarSVM, instant, homography, width, height, bikeCarSVM = None, pedBikeSpeedTreshold = float('Inf'), bikeCarSpeedThreshold = float('Inf'), px = 0.2, py = 0.2, pixelThreshold = 800):
520 fd9641cbd24b added function to classify object at instant from SVM Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 517 diff changeset	1209 '''Extract the image box around the object and
fd9641cbd24b added function to classify object at instant from SVM Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 517 diff changeset	1210 applies the SVM model on it'''
fd9641cbd24b added function to classify object at instant from SVM Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 517 diff changeset	1211 from numpy import array
fd9641cbd24b added function to classify object at instant from SVM Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 517 diff changeset	1212 croppedImg, yCropMin, yCropMax, xCropMin, xCropMax = imageBox(img, self, instant, homography, width, height, px, py, pixelThreshold)
fd9641cbd24b added function to classify object at instant from SVM Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 517 diff changeset	1213 if len(croppedImg) > 0: # != []
fd9641cbd24b added function to classify object at instant from SVM Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 517 diff changeset	1214 hog = array([cvutils.HOG(croppedImg)], dtype = np.float32)
522 ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1215 if self.aggregatedSpeed < pedBikeSpeedTreshold or bikeCarSVM == None:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1216 self.userTypes[instant] = int(pedBikeCarSVM.predict(hog))
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1217 elif self.aggregatedSpeed < bikeCarSpeedTreshold:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1218 self.userTypes[instant] = int(bikeCarSVM.predict(hog))
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1219 else:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1220 self.userTypes[instant] = userType2Num['car']
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1221 else:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1222 self.userTypes[instant] = userType2Num['unknown']
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1223
524 1dced8932b08 corrected bugs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 523 diff changeset	1224 def classifyUserTypeHoGSVM(self, images, pedBikeCarSVM, homography, width, height, bikeCarSVM = None, pedBikeSpeedTreshold = float('Inf'), bikeCarSpeedThreshold = float('Inf'), speedProbabilities = None, aggregationFunc = median, px = 0.2, py = 0.2, pixelThreshold = 800):
522 ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1225 '''Agregates SVM detections in each image and returns probability
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1226 (proportion of instants with classification in each category)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1227
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1228 iamges is a dictionary of images indexed by instant
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1229 With default parameters, the general (ped-bike-car) classifier will be used
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1230 TODO? consider all categories?'''
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1231 if not hasattr(self, aggregatedSpeed) or not hasattr(self, userTypes):
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1232 print('Initilize the data structures for classification by HoG-SVM')
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1233 self.initClassifyUserTypeHoGSVM(aggregationFunc)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1234
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1235 if len(self.userTypes) != self.length(): # if classification has not been done previously
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1236 for t in self.getTimeInterval():
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1237 if t not in self.userTypes:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1238 self.classifyUserTypeHoGSVMAtInstant(images[t], pedBikeCarSVM, t, homography, width, height, bikeCarSVM, pedBikeSpeedTreshold, bikeCarSpeedThreshold, px, py, pixelThreshold)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1239 # compute P(Speed\|Class)
524 1dced8932b08 corrected bugs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 523 diff changeset	1240 if speedProbabilities == None: # equiprobable information from speed
522 ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1241 userTypeProbabilities = {userType2Num['car']: 1., userType2Num['pedestrian']: 1., userType2Num['bicycle']: 1.}
520 fd9641cbd24b added function to classify object at instant from SVM Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 517 diff changeset	1242 else:
522 ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1243 userTypeProbabilities = {userType2Num[userTypename]: speedProbabilities[userTypename](self.aggregatedSpeed) for userTypename in speedProbabilities}
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1244 # result is P(Class\|Appearance) x P(Speed\|Class)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1245 nInstantsUserType = {userType2Num[userTypename]: 0 for userTypename in userTypeProbabilities}# number of instants the object is classified as userTypename
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1246 for t in self.userTypes:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1247 nInstantsUserType[self.userTypes[t]] += 1
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1248 for userTypename in userTypeProbabilities:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1249 userTypeProbabilities[userTypename] *= nInstantsUserType[userTypename]
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1250 # class is the user type that maximizes usertype probabilities
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1251 self.setUserType(utils.argmaxDict(userTypeProbabilities))
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1252
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1253 def classifyUserTypeArea(self, areas, homography):
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1254 '''Classifies the object based on its location (projected to image space)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1255 areas is a dictionary of matrix of the size of the image space
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1256 for different road users possible locations, indexed by road user type names
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1257
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1258 TODO: areas could be a wrapper object with a contains method that would work for polygons and images (with wrapper class)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1259 skip frames at beginning/end?'''
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1260 print('not implemented/tested yet')
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1261 if not hasattr(self, projectedPositions):
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1262 if homography != None:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1263 self.projectedPositions = obj.positions.project(homography)
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1264 else:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1265 self.projectedPositions = obj.positions
523 ce4eaabacc26 modified internal implementation of user type for classifyUserTypeArea Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 522 diff changeset	1266 possibleUserTypes = {userType: 0 for userType in range(len(userTypenames))}
522 ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1267 for p in self.projectedPositions:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1268 for userTypename in areas:
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1269 if areas[userTypename][p.x, p.y] != 0:
523 ce4eaabacc26 modified internal implementation of user type for classifyUserTypeArea Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 522 diff changeset	1270 possibleUserTypes[userType2Enum[userTypename]] += 1
522 ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1271 # what to do: threshold for most common type? self.setUserType()
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1272 return possibleUserTypes
520 fd9641cbd24b added function to classify object at instant from SVM Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 517 diff changeset	1273
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1274 @staticmethod
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1275 def collisionCourseDotProduct(movingObject1, movingObject2, instant):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1276 'A positive result indicates that the road users are getting closer'
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1277 deltap = movingObject1.getPositionAtInstant(instant)-movingObject2.getPositionAtInstant(instant)
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1278 deltav = movingObject2.getVelocityAtInstant(instant)-movingObject1.getVelocityAtInstant(instant)
148 ad21db62b785 bug correction for cosine functions in python Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 115 diff changeset	1279 return Point.dot(deltap, deltav)
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1280
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1281 @staticmethod
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1282 def collisionCourseCosine(movingObject1, movingObject2, instant):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1283 'A positive result indicates that the road users are getting closer'
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1284 return Point.cosine(movingObject1.getPositionAtInstant(instant)-movingObject2.getPositionAtInstant(instant), #deltap
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1285 movingObject2.getVelocityAtInstant(instant)-movingObject1.getVelocityAtInstant(instant)) #deltav
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1286
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1287 def plotRoadUsers(objects, colors):
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1288 '''Colors is a PlottingPropertyValues instance'''
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1289 from matplotlib.pyplot import figure, axis
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1290 figure()
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1291 for obj in objects:
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	1292 obj.plot(colors.get(obj.userType))
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1293 axis('equal')
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1294
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1295
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1296 if __name__ == "__main__":
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1297 import doctest
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1298 import unittest
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1299 suite = doctest.DocFileSuite('tests/moving.txt')
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1300 #suite = doctest.DocTestSuite()
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1301 unittest.TextTestRunner().run(suite)
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1302 #doctest.testmod()
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1303 #doctest.testfile("example.txt")

Mercurial Hosting > traffic-intelligence

annotate python/moving.py @ 578:fe4e9d2b807d