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

annotate python/moving.py @ 630:69a98f84f3eb

corrected major issue with pPET, only for CVDirect prediction for now

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Tue, 17 Feb 2015 02:21:31 +0100
parents	82e9f78a4714
children	2d1d33ae1c69

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
623 ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	198 def divide(self, alpha):
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	199 'Warning, returns a new Point'
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	200 return Point(self.x/alpha, self.y/alpha)
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	201
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	202 def plot(self, options = 'o', **kwargs):
41 eb78c6edc0c8 added drawing for Point Nicolas Saunier <nico@confins.net> parents: 40 diff changeset	203 from matplotlib.pylab import plot
211 ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	204 plot([self.x], [self.y], options, **kwargs)
41 eb78c6edc0c8 added drawing for Point Nicolas Saunier <nico@confins.net> parents: 40 diff changeset	205
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	206 def norm2Squared(self):
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	207 '''2-norm distance (Euclidean distance)'''
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	208 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	209
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	210 def norm2(self):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	211 '''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	212 return sqrt(self.norm2Squared())
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	213
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	214 def norm1(self):
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	215 return abs(self.x)+abs(self.y)
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	216
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	217 def normMax(self):
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	218 return max(abs(self.x),abs(self.y))
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	219
64 c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	220 def aslist(self):
c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	221 return [self.x, self.y]
c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	222
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	223 def astuple(self):
e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	224 return (self.x, self.y)
e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	225
223 c31722fcc9de minor modifications for integer drawing in OpenCV Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 214 diff changeset	226 def asint(self):
c31722fcc9de minor modifications for integer drawing in OpenCV Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 214 diff changeset	227 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	228
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	229 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	230 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	231 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	232
98 b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	233 def project(self, homography):
b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	234 from numpy.core.multiarray import array
b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	235 projected = cvutils.projectArray(homography, array([[self.x], [self.y]]))
b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	236 return Point(projected[0], projected[1])
b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	237
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	238 def inPolygon(self, polygon):
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	239 '''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	240 (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	241
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	242 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	243
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	244 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	245
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	246 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	247 counter = 0;
5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	248
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	249 p1 = polygon[0,:];
79 5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	250 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	251 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	252 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	253 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	254 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	255 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	256 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	257 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	258 counter+=1;
5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	259 p1=p2
5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	260 return (counter%2 == 1);
5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	261
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	262 @staticmethod
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	263 def fromList(p):
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	264 return Point(p[0], p[1])
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	265
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	266 @staticmethod
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	267 def dot(p1, p2):
89 f88a19695bba added inner product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 83 diff changeset	268 'Scalar product'
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	269 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	270
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	271 @staticmethod
90 f84293ad4611 renamed inner to cross product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 89 diff changeset	272 def cross(p1, p2):
f84293ad4611 renamed inner to cross product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 89 diff changeset	273 'Cross product'
89 f88a19695bba added inner product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 83 diff changeset	274 return p1.xp2.y-p1.yp2.x
f88a19695bba added inner product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 83 diff changeset	275
f88a19695bba added inner product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 83 diff changeset	276 @staticmethod
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	277 def cosine(p1, p2):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	278 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	279
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	280 @staticmethod
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	281 def distanceNorm2(p1, p2):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	282 return (p1-p2).norm2()
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	283
64 c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	284 @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	285 def plotAll(points, **kwargs):
64 c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	286 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	287 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	288
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	289 def similarOrientation(self, refDirection, cosineThreshold):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	290 '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	291 return Point.cosine(self, refDirection) >= cosineThreshold
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	292
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	293 @staticmethod
484 6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	294 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	295 '''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	296 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	297 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	298 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	299 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	300 dv = v1-v2
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	301 dp = p1-p2
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	302 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	303 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	304 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	305
504 a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	306 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	307 if delta >= 0:
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	308 deltaRoot = sqrt(delta)
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	309 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	310 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	311 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	312 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	313 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	314 ttc = ttc1
f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 527 diff changeset	315 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	316 ttc = ttc2
f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 527 diff changeset	317 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	318 ttc = None
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	319 else:
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	320 ttc = None
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	321 return ttc
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	322
583 6ebfb43e938e added midpoint function (from laurent gauthier) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 582 diff changeset	323 @staticmethod
6ebfb43e938e added midpoint function (from laurent gauthier) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 582 diff changeset	324 def midPoint(p1, p2):
6ebfb43e938e added midpoint function (from laurent gauthier) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 582 diff changeset	325 'Returns the middle of the segment [p1, p2]'
6ebfb43e938e added midpoint function (from laurent gauthier) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 582 diff changeset	326 return Point(0.5p1.x+0.5p2.x, 0.5p1.y+0.5p2.y)
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	327
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	328 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	329 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	330 '''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	331 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	332 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	333
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	334 # Functions for coordinate transformation
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	335 # 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	336 def subsec_spline_dist(splines):
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	337 ''' 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	338
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	339 Output:
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 ss_spline_d[spline #][mode][station]
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	342
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	343 where:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	344 mode=0: incremental distance
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	345 mode=1: cumulative distance
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	346 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	347 '''
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	348
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	349 from numpy import zeros
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	350 ss_spline_d = []
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	351 #Prepare subsegment distances
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	352 for spline in range(len(splines)):
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	353 ss_spline_d.append([[],[],[]])
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	354 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	355 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	356 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	357 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	358 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	359 break
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	360 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	361 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	362 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	363
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	364 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	365
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	366 return ss_spline_d
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	367
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	368 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	369 ''' 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	370 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	371 '''
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	372 if(p0x == p1x and p0y == p1y):
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	373 return None
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	374 try:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	375 #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	376 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	377 p1x += 0.0000000001
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	378 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	379 p1y += 0.0000000001
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	380 #make the calculation
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	381 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	382 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	383 except ZeroDivisionError:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	384 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	385 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	386 import pdb; pdb.set_trace()
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	387 return Point(X,Y)
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	388
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	389 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5):
579 05c927c6d3cf curvilinear projection seems operational Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 578 diff changeset	390 ''' Snap a point p to it's nearest subsegment of it's nearest spline (from the list splines). A spline is a list of points (class Point), most likely a trajectory.
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	391
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	392 Output:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	393 =======
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	394 [spline index,
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	395 subsegment leading point index,
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	396 snapped point,
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	397 subsegment distance,
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	398 spline distance,
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	399 orthogonal point offset]
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	400 '''
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	401 minOffsetY = float('inf')
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	402 #For each spline
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	403 for spline in range(len(splines)):
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	404 #For each spline point index
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	405 for spline_p in range(len(splines[spline])-1):
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	406 #Get closest point on spline
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	407 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	408 if closestPoint == None:
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	409 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	410 return None
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	411 # check if the
fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	412 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	413 offsetY = Point.distanceNorm2(closestPoint, p)
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	414 if offsetY < minOffsetY:
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	415 minOffsetY = offsetY
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	416 snappedSpline = spline
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	417 snappedSplineLeadingPoint = spline_p
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	418 snappedPoint = Point(closestPoint.x, closestPoint.y)
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	419 #Jump loop if significantly close
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	420 if offsetY < goodEnoughSplineDistance:
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	421 break
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	422 #Get sub-segment distance
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	423 if minOffsetY != float('inf'):
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	424 subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSpline][snappedSplineLeadingPoint])
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	425 #Get cumulative alignment distance (total segment distance)
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	426 splineDistanceS = splines[snappedSpline].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	427 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	428 offsetVector = p-snappedPoint
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	429 if Point.dot(orthogonalSplineVector, offsetVector) < 0:
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	430 minOffsetY = -minOffsetY
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	431 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	432 else:
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	433 return None
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	434
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	435 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	436 ''' Find X,Y coordinate from S,Y data.
568 538fb47b3007 minor modification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	437 if mode = 0 : return Snapped X,Y
538fb47b3007 minor modification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	438 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	439 '''
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	440
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	441 #(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	442 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	443
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	444 #Find subsegment
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	445 snapped_x = None
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	446 snapped_y = None
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	447 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	448 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	449 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	450 #Get normal vector and then snap
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	451 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	452 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	453 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	454 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	455 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	456 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	457 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	458
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	459 #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	460 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	461 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	462 break
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	463
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	464 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	465 if(not snapped_x):
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	466 snapped_x = splines[splineNum][-1][0]
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	467 snapped_y = splines[splineNum][-1][1]
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	468 return [snapped_x,snapped_y]
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	469 else:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	470 return [real_x,real_y]
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	471
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	472
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	473 class NormAngle(object):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	474 '''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	475
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	476 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	477 self.norm = norm
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	478 self.angle = angle
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	479
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	480 @staticmethod
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	481 def fromPoint(p):
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	482 from math import atan2
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	483 norm = p.norm2()
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	484 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	485 angle = atan2(p.y, p.x)
339 9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	486 else:
9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	487 angle = 0.
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	488 return NormAngle(norm, angle)
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	489
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	490 def __add__(self, other):
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	491 '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	492 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	493
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	494 def getPoint(self):
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	495 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	496 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	497
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	498
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	499 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	500 '''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	501 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	502
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	503 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	504 '''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	505 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	506 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	507 predictedSpeedTheta = speedOrientation+control
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	508 if maxSpeed:
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	509 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	510 predictedPosition = position+predictedSpeedTheta.getPoint()
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	511 return predictedPosition, predictedSpeedTheta
245 bd8ab323c198 corrected issue with predictPosiont static method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 244 diff changeset	512
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	513
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	514 class FlowVector(object):
184 d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	515 '''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	516 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	517 def __init__(self, position, velocity):
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	518 '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	519 self.position = position
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	520 self.velocity = velocity
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	521
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	522 def __add__(self, other):
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	523 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	524
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	525 def multiply(self, alpha):
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	526 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	527
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	528 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	529 from matplotlib.pylab import plot
211 ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	530 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	531 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	532
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	533 @staticmethod
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	534 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	535 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	536
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	537 def intersection(p1, p2, p3, p4):
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	538 ''' 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	539 http://paulbourke.net/geometry/pointlineplane/'''
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	540 dp12 = p2-p1
571 a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	541 dp34 = p4-p3
a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	542 #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	543 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	544 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	545 return None
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	546 else:
571 a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	547 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	548 return p1+dp12.multiply(ua)
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	549
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	550 # def intersection(p1, p2, dp1, dp2):
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	551 # '''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	552 # 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	553 # from numpy import matrix
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	554 # from numpy.linalg import linalg
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	555 # A = matrix([[dp1.y, -dp1.x],
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	556 # [dp2.y, -dp2.x]])
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	557 # 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	558 # [dp2.yp2.x-dp2.xp2.y]])
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	559
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	560 # if linalg.det(A) == 0:
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	561 # return None
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	562 # else:
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	563 # intersection = linalg.solve(A,B)
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	564 # 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	565
152 74b1fc68d4df re-organized code to avoid cyclic python module dependencies Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 148 diff changeset	566 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	567 '''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	568
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	569 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	570 return None
74b1fc68d4df re-organized code to avoid cyclic python module dependencies Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 148 diff changeset	571 else:
571 a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	572 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	573 if (inter != None
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	574 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	575 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	576 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	577 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	578 return inter
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	579 else:
258 d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	580 return None
d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	581
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	582 def segmentLineIntersection(p1, p2, p3, p4):
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	583 '''Indicates if the line going through p1 and p2 intersects inside p3, p4'''
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	584 inter = intersection(p1, p2, p3, p4)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	585 if inter != None and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p3.y, p4.y, inter.y)):
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	586 return inter
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	587 else:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	588 return None
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	589
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	590
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	591 class Trajectory(object):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	592 '''Class for trajectories: temporal sequence of positions
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	593
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	594 The class is iterable'''
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	595
79 5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	596 def __init__(self, positions=None):
331 40790d93200e corrected a bug for Trajectory initialization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 329 diff changeset	597 if positions != None:
113 606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	598 self.positions = positions
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	599 else:
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	600 self.positions = [[],[]]
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	601
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	602 @staticmethod
555 f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	603 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	604 t = Trajectory()
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	605 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	606 t.addPosition(p0)
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	607 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	608 p0 += v
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	609 t.addPosition(p0)
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	610 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	611
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	612 @staticmethod
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	613 def load(line1, line2):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	614 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	615 [float(n) for n in line2.split(' ')]])
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	616
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	617 @staticmethod
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	618 def fromPointList(points):
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	619 t = Trajectory()
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	620 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	621 for p in points:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	622 t.addPositionXY(p[0],p[1])
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	623 else:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	624 for p in points:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	625 t.addPosition(p)
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	626 return t
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	627
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	628 def __len__(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	629 return len(self.positions[0])
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	630
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	631 def length(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	632 return self.__len__()
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	633
582 7e1ae4d97f1a corrected bug for curvilinear trajectory with only one position and differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 581 diff changeset	634 def empty(self):
7e1ae4d97f1a corrected bug for curvilinear trajectory with only one position and differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 581 diff changeset	635 return self.__len__() == 0
7e1ae4d97f1a corrected bug for curvilinear trajectory with only one position and differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 581 diff changeset	636
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	637 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	638 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	639 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	640 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	641 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	642 #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	643
16 9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	644 def __str__(self):
39 e47168f6b694 corrected printing bug Nicolas Saunier <nico@confins.net> parents: 38 diff changeset	645 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	646
69 cc192d0450b3 added full support for two implementations of indicators, with tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 68 diff changeset	647 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	648 return self.__str__()
23 5f2921ad4f7e made Trajectory indexable and timeinterval iterable Nicolas Saunier <nico@confins.net> parents: 22 diff changeset	649
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	650
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	651 def __iter__(self):
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	652 self.iterInstantNum = 0
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	653 return self
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	654
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	655 def next(self):
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	656 if self.iterInstantNum >= self.length():
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	657 raise StopIteration
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	658 else:
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	659 self.iterInstantNum += 1
23 5f2921ad4f7e made Trajectory indexable and timeinterval iterable Nicolas Saunier <nico@confins.net> parents: 22 diff changeset	660 return self[self.iterInstantNum-1]
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	661
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	662 def setPositionXY(self, i, x, y):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	663 if i < self.__len__():
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	664 self.positions[0][i] = x
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	665 self.positions[1][i] = y
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	666
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	667 def setPosition(self, i, p):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	668 self.setPositionXY(i, p.x, p.y)
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	669
71 45e958ccd9bd added new addPosition method to Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 69 diff changeset	670 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	671 self.positions[0].append(x)
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	672 self.positions[1].append(y)
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	673
71 45e958ccd9bd added new addPosition method to Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 69 diff changeset	674 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	675 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	676
343 74e437ab5f11 first version of indicator loading code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 339 diff changeset	677 def duplicateLastPosition(self):
74e437ab5f11 first version of indicator loading code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 339 diff changeset	678 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	679 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	680
182 d3f6de6c3918 added drawing functialities, in particular on aerial image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 152 diff changeset	681 @staticmethod
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	682 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	683 from matplotlib.pylab import plot
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	684 if lastCoordinate == None:
270 05c9b0cb8202 updates for drawing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 268 diff changeset	685 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	686 elif 0 <= lastCoordinate <= len(positions[0]):
270 05c9b0cb8202 updates for drawing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 268 diff changeset	687 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	688 if withOrigin:
211 ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	689 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	690
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	691 def project(self, homography):
622 dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	692 return Trajectory(cvutils.projectTrajectory(homography, self.positions).tolist())
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	693
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	694 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	695 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	696
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	697 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	698 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	699
620 aee4cbac9e0e corrected bug to plot trajectories on world image (the norm on image coordinates seems to have changed in matplotlib) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 595 diff changeset	700 def plotOnWorldImage(self, nPixelsPerUnitDistance, 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	701 imgPositions = [[x*nPixelsPerUnitDistance for x in self.positions[0]],
620 aee4cbac9e0e corrected bug to plot trajectories on world image (the norm on image coordinates seems to have changed in matplotlib) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 595 diff changeset	702 [x*nPixelsPerUnitDistance for x in self.positions[1]]]
aee4cbac9e0e corrected bug to plot trajectories on world image (the norm on image coordinates seems to have changed in matplotlib) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 595 diff changeset	703 Trajectory._plot(imgPositions, options, withOrigin, None, timeStep, **kwargs)
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	704
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	705 def getXCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	706 return self.positions[0]
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	707
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	708 def getYCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	709 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	710
a5ef9e40688e makes use of matplotlib function to test if point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 91 diff changeset	711 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	712 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	713 return array(self.positions)
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	714
14 e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	715 def xBounds(self):
e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	716 # 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	717 return Interval(min(self.getXCoordinates()), max(self.getXCoordinates()))
14 e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	718
e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	719 def yBounds(self):
e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	720 # 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	721 return Interval(min(self.getYCoordinates()), max(self.getYCoordinates()))
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	722
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	723 def add(self, traj2):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	724 '''Returns a new trajectory of the same length'''
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	725 if self.length() != traj2.length():
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	726 print 'Trajectories of different lengths'
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	727 return None
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	728 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	729 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	730 [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	731
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	732 def subtract(self, traj2):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	733 '''Returns a new trajectory of the same length'''
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	734 if self.length() != traj2.length():
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	735 print 'Trajectories of different lengths'
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	736 return None
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	737 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	738 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	739 [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	740
590 0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	741 def multiply(self, alpha):
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	742 '''Returns a new trajectory of the same length'''
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	743 return Trajectory([[alpha*x for x in self.getXCoordinates()],
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	744 [alpha*y for y in self.getYCoordinates()]])
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	745
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	746 def differentiate(self, doubleLastPosition = False):
339 9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	747 diff = Trajectory()
9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	748 for i in xrange(1, self.length()):
9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	749 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	750 if doubleLastPosition:
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	751 diff.addPosition(diff[-1])
339 9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	752 return diff
9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	753
16 9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	754 def norm(self):
9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	755 '''Returns the list of the norms at each instant'''
9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	756 # def add(x, y): return x+y
9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	757 # 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	758 # return sqrt(sq)
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	759 from numpy import hypot
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	760 return hypot(self.positions[0], self.positions[1])
14 e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	761
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	762 # def cumulatedDisplacement(self):
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	763 # '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	764 # displacement = 0
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	765 # for i in xrange(self.length()-1):
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	766 # 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	767 # return displacement
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	768
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	769 def computeCumulativeDistances(self):
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	770 '''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	771 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	772 self.distances = []
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	773 self.cumulativeDistances = [0.]
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	774 p1 = self[0]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	775 cumulativeDistance = 0.
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	776 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	777 p2 = self[i+1]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	778 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	779 cumulativeDistance += self.distances[-1]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	780 self.cumulativeDistances.append(cumulativeDistance)
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	781 p1 = p2
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	782
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	783 def getDistance(self,i):
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	784 '''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	785 if i < self.length()-1:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	786 return self.distances[i]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	787 else:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	788 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	789
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	790 def getCumulativeDistance(self, i):
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	791 '''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	792 if i < self.length():
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	793 return self.cumulativeDistances[i]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	794 else:
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	795 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	796
381 387cc0142211 script to replay event annotations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 379 diff changeset	797 def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5):
387cc0142211 script to replay event annotations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 379 diff changeset	798 '''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	799 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	800 count = 0
383 0ce2210790b1 fixed stupid naming bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 381 diff changeset	801 lengthThreshold = float(self.length())*minProportion
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	802 for p in self:
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	803 if p.similarOrientation(refDirection, cosineThreshold):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	804 count += 1
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	805 return count >= lengthThreshold
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	806
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	807 def wiggliness(self):
608 078adacd72a4 moving.py integrating Mohamed's comments refactored Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 607 diff changeset	808 return self.getCumulativeDistance(self.length()-1)/float(Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1)))
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	809
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	810 def getIntersections(self, p1, p2):
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	811 '''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	812 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	813 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	814 indices = []
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	815
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	816 for i in xrange(self.length()-1):
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	817 q1=self.__getitem__(i)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	818 q2=self.__getitem__(i+1)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	819 p = utils.segmentIntersection(q1, q2, p1, p2)
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	820 if p != None:
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	821 if q1.x != q2.x:
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	822 ratio = (p.x-q1.x)/(q2.x-q1.x)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	823 elif q1.y != q2.y:
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	824 ratio = (p.y-q1.y)/(q2.y-q1.y)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	825 else:
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	826 ratio = 0
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	827 indices.append(i+ratio)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	828 return indices
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	829
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	830 def getLineIntersections(self, p1, p2):
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	831 '''Returns a list of the indices at which the trajectory
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	832 intersects with the segment of extremities p1 and p2
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	833 the list is empty if there is no crossing'''
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	834 indices = []
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	835 intersections = []
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	836
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	837 for i in xrange(self.length()-1):
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	838 q1=self.__getitem__(i)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	839 q2=self.__getitem__(i+1)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	840 p = utils.segmentLineIntersection(p1, p2, q1, q2)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	841 if p != None:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	842 if q1.x != q2.x:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	843 ratio = (p.x-q1.x)/(q2.x-q1.x)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	844 elif q1.y != q2.y:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	845 ratio = (p.y-q1.y)/(q2.y-q1.y)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	846 else:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	847 ratio = 0
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	848 indices.append(i+ratio)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	849 intersections.append(p)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	850 return indices, intersections
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	851
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	852 def getTrajectoryInInterval(self, inter):
622 dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	853 'Returns all position between index inter.first and index.last (included)'
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	854 if inter.first >=0 and inter.last<= self.length():
622 dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	855 return Trajectory([self.positions[0][inter.first:inter.last+1],
dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	856 self.positions[1][inter.first:inter.last+1]])
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	857 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	858 return None
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	859
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	860 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	861 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	862 '''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	863 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	864 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	865 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	866 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	867 return traj
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	868
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	869 def proportionInPolygon(self, polygon, minProportion = 0.5):
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	870 pointsIn = pointsInPolygon([p.asShapely() for p in self], polygon)
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	871 lengthThreshold = float(self.length())*minProportion
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	872 return len(pointsIn) >= lengthThreshold
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	873 else:
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	874 def getTrajectoryInPolygon(self, polygon):
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	875 '''Returns the trajectory built with the set of points inside the polygon
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	876 (array of Nx2 coordinates of the polygon vertices)'''
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	877 traj = Trajectory()
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	878 for p in self:
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	879 if p.inPolygon(polygon):
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	880 traj.addPosition(p)
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	881 return traj
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	882
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	883 def proportionInPolygon(self, polygon, minProportion = 0.5):
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	884 pointsInPolygon = [p.inPolygon(polygon) for p in self]
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	885 lengthThreshold = float(self.length())*minProportion
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	886 return len(pointsInPolygon) >= lengthThreshold
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	887
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	888 @staticmethod
369 027e254f0b53 lcss subclass for indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 368 diff changeset	889 def lcss(t1, t2, lcss):
027e254f0b53 lcss subclass for indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 368 diff changeset	890 return lcss.compute(t1, t2)
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	891
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	892 class CurvilinearTrajectory(Trajectory):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	893 '''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	894 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	895 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	896
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	897 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	898 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	899 self.positions = [[],[]]
552 ca6bded754ac corrected more bugs from merging (caught in regression tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 551 diff changeset	900 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	901 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	902 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	903 self.positions = [S,Y]
543 cb213269d330 defensive code for curvilinear trajectory initialization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 542 diff changeset	904 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	905 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	906 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	907 self.lanes = lanes
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	908
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	909 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	910 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	911 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	912 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	913 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	914 #elif isinstance( key, slice ):
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	915
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	916 def getSCoordinates(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	917 return self.getXCoordinates()
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	918
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	919 def getLanes(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	920 return self.lanes
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	921
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	922 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	923 self.addPositionXY(s,y)
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	924 self.lanes.append(lane)
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	925
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	926 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	927 '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	928 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	929
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	930 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	931 self.setPositionXY(i, s, y)
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	932 if i < self.__len__():
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	933 self.lanes[i] = lane
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	934
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	935 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	936 diff = CurvilinearTrajectory()
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	937 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	938 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	939 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	940 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	941 p1=p2
582 7e1ae4d97f1a corrected bug for curvilinear trajectory with only one position and differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 581 diff changeset	942 if doubleLastPosition and self.length() > 1:
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	943 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	944 return diff
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	945
451 cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	946 def getIntersections(self, S1, lane = None):
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	947 '''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	948 goes past the curvilinear coordinate S1
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	949 (in provided lane if lane != None)
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	950 the list is empty if there is no crossing'''
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	951 indices = []
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	952 for i in xrange(self.length()-1):
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	953 q1=self.__getitem__(i)
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	954 q2=self.__getitem__(i+1)
527 37830a831818 loading from VISSIM trajectory data works Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 526 diff changeset	955 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	956 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	957 return indices
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	958
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	959 ##################
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	960 # Moving Objects
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	961 ##################
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	962
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	963 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	964 'car',
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	965 '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	966 'motorcycle',
a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	967 'bicycle',
185 c06379f25ab8 utilities for user types Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 184 diff changeset	968 'bus',
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	969 'truck']
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	970
185 c06379f25ab8 utilities for user types Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 184 diff changeset	971 userType2Num = utils.inverseEnumeration(userTypeNames)
c06379f25ab8 utilities for user types Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 184 diff changeset	972
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	973 class MovingObject(STObject):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	974 '''Class for moving objects: a spatio-temporal object
588 c5406edbcf12 added loading ground truth annotations (ground truth) from polytrack format Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 587 diff changeset	975 with a trajectory and a geometry (constant volume over time)
c5406edbcf12 added loading ground truth annotations (ground truth) from polytrack format Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 587 diff changeset	976 and a usertype (e.g. road user) coded as a number (see userTypeNames)
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	977 '''
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	978
411 31604ef1cad4 added hog functions and the display of road user types if known Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 383 diff changeset	979 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	980 super(MovingObject, self).__init__(num, timeInterval)
16 9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	981 self.positions = positions
345 fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	982 self.velocities = velocities
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	983 self.geometry = geometry
54 c354d41ef7cd corrected code for usertype in movingobject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 49 diff changeset	984 self.userType = userType
517 47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	985 self.features = []
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	986 # 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	987
555 f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	988 @staticmethod
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	989 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	990 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	991 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	992
622 dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	993 @staticmethod
623 ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	994 def concatenate(obj1, obj2, num = None):
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	995 '''Concatenates two objects supposed to overlap temporally '''
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	996 commonTimeInterval = obj1.commonTimeInterval(obj2)
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	997 if commonTimeInterval.empty():
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	998 print('The two objects\' time intervals do not overlap: obj1 {} and obj2 {}'.format(obj1.getTimeInterval(), obj2.getTimeInterval()))
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	999 return None
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1000 else:
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1001 if num == None:
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1002 newNum = obj1.getNum()
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1003 else:
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1004 newNum = num
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1005 newTimeInterval = TimeInterval.union(obj1.getTimeInterval(), obj2.getTimeInterval())
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1006 # positions
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1007 positions = Trajectory()
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1008 for t in newTimeInterval:
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1009 p = Point(0.,0.)
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1010 n = 0.
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1011 if obj1.existsAtInstant(t):
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1012 p += obj1.getPositionAtInstant(t)
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1013 n += 1.
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1014 if obj2.existsAtInstant(t):
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1015 p += obj2.getPositionAtInstant(t)
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1016 n += 1.
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1017 assert n>0, 'there should be at least one point for each instant'
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1018 positions.addPosition(p.divide(n))
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1019 # velocities: if any
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1020 if hasattr(obj1, 'velocities') and hasattr(obj2, 'velocities'):
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1021 velocities = Trajectory()
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1022 for t in newTimeInterval:
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1023 p = Point(0.,0.)
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1024 n = 0.
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1025 if obj1.existsAtInstant(t):
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1026 p += obj1.getVelocityAtInstant(t)
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1027 n += 1.
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1028 if obj2.existsAtInstant(t):
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1029 p += obj2.getVelocityAtInstant(t)
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1030 n += 1.
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1031 assert n>0, 'there should be at least one point for each instant'
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1032 velocities.addPosition(p.divide(n))
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1033 else:
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1034 velocities = None
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1035 # TODO object envelop (polygon)
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1036 # user type
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1037 if obj1.getUserType() != obj2.getUserType():
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1038 print('The two moving objects have different user types: obj1 {} obj2 {}'.format(userTypeNames[obj1.getUserType()], userTypeNames[obj2.getUserType()]))
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1039
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1040 return MovingObject(newNum, newTimeInterval, positions, velocities, userType = obj1.getUserType())
622 dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	1041
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1042 def getObjectInTimeInterval(self, inter):
54 c354d41ef7cd corrected code for usertype in movingobject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 49 diff changeset	1043 '''Returns a new object extracted from self,
c354d41ef7cd corrected code for usertype in movingobject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 49 diff changeset	1044 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	1045 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	1046 if not intersection.empty():
b3a1c26e2f22 corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 96 diff changeset	1047 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	1048 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	1049 if self.velocities:
97 b3a1c26e2f22 corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 96 diff changeset	1050 obj.velocities = self.velocities.getTrajectoryInInterval(trajectoryInterval)
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1051 return obj
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1052 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1053 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	1054 return None
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1055
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1056 def length(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1057 return self.timeInterval.length()
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1058
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1059 def getPositions(self):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1060 return self.positions
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1061
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1062 def getVelocities(self):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1063 return self.velocities
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1064
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	1065 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	1066 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	1067
335 3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1068 def getCurvilinearPositions(self):
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1069 if hasattr(self, 'curvilinearPositions'):
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1070 return self.curvilinearPositions
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1071 else:
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1072 return None
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1073
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	1074 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	1075 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	1076
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 259 diff changeset	1077 def setFeatures(self, features):
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 259 diff changeset	1078 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	1079
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	1080 def getSpeeds(self):
1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	1081 return self.getVelocities().norm()
1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	1082
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1083 def getPositionAt(self, i):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1084 return self.positions[i]
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1085
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1086 def getVelocityAt(self, i):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1087 return self.velocities[i]
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1088
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1089 def getPositionAtInstant(self, i):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1090 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	1091
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1092 def getVelocityAtInstant(self, i):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1093 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	1094
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1095 def getXCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1096 return self.positions.getXCoordinates()
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1097
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1098 def getYCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1099 return self.positions.getYCoordinates()
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1100
517 47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1101 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	1102 if withFeatures:
47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1103 for f in self.features:
47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1104 f.positions.plot('r', True, timeStep, **kwargs)
47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1105 self.positions.plot('bx-', True, timeStep, **kwargs)
47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1106 else:
47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1107 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	1108
621 582508610572 merge and updated consistently that imageheight does not seem necessary anymore for plotting on world image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 619 620 diff changeset	1109 def plotOnWorldImage(self, nPixelsPerUnitDistance, options = '', withOrigin = False, timeStep = 1, **kwargs):
582508610572 merge and updated consistently that imageheight does not seem necessary anymore for plotting on world image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 619 620 diff changeset	1110 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, **kwargs)
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	1111
626 35155ac2a294 corrected bugs, in particular to MovingObject.play Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 624 diff changeset	1112 def play(self, videoFilename, homography = None, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1.):
35155ac2a294 corrected bugs, in particular to MovingObject.play Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 624 diff changeset	1113 cvutils.displayTrajectories(videoFilename, [self], homography = homography, firstFrameNum = self.getFirstInstant(), lastFrameNumArg = self.getLastInstant(), undistort = undistort, intrinsicCameraMatrix = intrinsicCameraMatrix, distortionCoefficients = distortionCoefficients, undistortedImageMultiplication = undistortedImageMultiplication)
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	1114
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1115 def speedDiagnostics(self, framerate = 1., display = False):
377 2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1116 from numpy import std
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1117 from scipy.stats import scoreatpercentile
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1118 speeds = framerate*self.getSpeeds()
377 2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1119 coef = utils.linearRegression(range(len(speeds)), speeds)
379 f1a1923ddff9 minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 378 diff changeset	1120 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	1121 if display:
379 f1a1923ddff9 minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 378 diff changeset	1122 from matplotlib.pyplot import figure, plot, axis
377 2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1123 figure(1)
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	1124 self.plot()
379 f1a1923ddff9 minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 378 diff changeset	1125 axis('equal')
377 2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1126 figure(2)
2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1127 plot(list(self.getTimeInterval()), speeds)
2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1128
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1129 @staticmethod
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1130 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	1131 from scipy.spatial.distance import cdist
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1132 if _instant2 == None:
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1133 instant2 = instant1
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1134 else:
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1135 instant2 = _instant2
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1136 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	1137 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	1138 return cdist(positions1, positions2, metric = 'euclidean')
6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1139
6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1140 @staticmethod
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1141 def minDistance(obj1, obj2, instant1, instant2 = None):
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1142 return MovingObject.distances(obj1, obj2, instant1, instant2).min()
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1143
6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1144 @staticmethod
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1145 def maxDistance(obj1, obj2, instant, instant2 = None):
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1146 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	1147
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1148 def maxSize(self):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1149 '''Returns the max distance between features
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1150 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	1151 if hasattr(self, 'features'):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1152 nFeatures = -1
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1153 tMaxFeatures = 0
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1154 for t in self.getTimeInterval():
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1155 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	1156 if n > nFeatures:
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1157 nFeatures = n
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1158 tMaxFeatures = t
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1159 return MovingObject.maxDistance(self, self, tMaxFeatures)
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1160 else:
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1161 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	1162 return None
619 dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 596 616 diff changeset	1163
550 5668af2ff515 minor naming Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 549 diff changeset	1164 def setRoutes(self, startRouteID, endRouteID):
5668af2ff515 minor naming Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 549 diff changeset	1165 self.startRouteID = startRouteID
5668af2ff515 minor naming Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 549 diff changeset	1166 self.endRouteID = endRouteID
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1167
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1168 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	1169 '''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	1170 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	1171 empty list if there is no crossing'''
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1172 indices = self.positions.getIntersections(p1, p2)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1173 return [t+self.getFirstInstant() for t in indices]
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	1174
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	1175 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	1176 '''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	1177 at constant speed'''
255 13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	1178 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	1179
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1180 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	1181 ''' 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	1182 (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	1183 curvilinear coordinates using alignment metadata.
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1184 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	1185 ======
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1186
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1187 Input:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1188 ======
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1189 alignments = a list of alignments, where each alignment is a list of
579 05c927c6d3cf curvilinear projection seems operational Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 578 diff changeset	1190 points (class Point).
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1191 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	1192 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	1193 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	1194
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1195 '''
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1196
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1197 self.curvilinearPositions = CurvilinearTrajectory()
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1198
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1199 #For each point
579 05c927c6d3cf curvilinear projection seems operational Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 578 diff changeset	1200 for i in xrange(int(self.length())):
05c927c6d3cf curvilinear projection seems operational Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 578 diff changeset	1201 result = getSYfromXY(self.getPositionAt(i), alignments)
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1202
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1203 # Error handling
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1204 if(result == None):
579 05c927c6d3cf curvilinear projection seems operational Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 578 diff changeset	1205 print('Warning: trajectory {} at point {} {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.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	1206 else:
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1207 [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	1208 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	1209
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1210 ## 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	1211 #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	1212 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	1213 ## Recalculate projected point to new lane
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1214 lanes = self.curvilinearPositions.getLanes()
fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1215 if(lanes != smoothed_lanes):
579 05c927c6d3cf curvilinear projection seems operational Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 578 diff changeset	1216 for i in xrange(len(lanes)):
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1217 if(lanes[i] != smoothed_lanes[i]):
fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1218 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	1219
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1220 # Error handling
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1221 if(result == None):
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1222 ## 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	1223 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	1224 else:
579 05c927c6d3cf curvilinear projection seems operational Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 578 diff changeset	1225 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1226 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	1227
562 259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1228 def computeSmoothTrajectory(self, minCommonIntervalLength):
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1229 '''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	1230 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	1231
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1232 Warning work in progress
562 259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1233 TODO? not use the first/last 1-.. positions'''
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1234 from numpy import array, median
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1235 nFeatures = len(self.features)
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1236 if nFeatures == 0:
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1237 print('Empty object features\nCannot compute smooth trajectory')
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1238 else:
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1239 # compute the relative position vectors
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1240 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	1241 for i in xrange(nFeatures):
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1242 for j in xrange(i):
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1243 fi = self.features[i]
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1244 fj = self.features[j]
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1245 inter = fi.commonTimeInterval(fj)
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1246 if inter.length() >= minCommonIntervalLength:
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1247 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	1248 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	1249 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	1250 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	1251 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	1252 relativePositions[(j,i)] = -relativePositions[(i,j)]
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1253
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	1254 ###
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 # 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	1256 ###
524 1dced8932b08 corrected bugs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 523 diff changeset	1257 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	1258 '''Classifies slow and fast road users
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1259 slow: non-motorized -> pedestrians
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 583 diff changeset	1260 fast: motorized -> cars
608 078adacd72a4 moving.py integrating Mohamed's comments refactored Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 607 diff changeset	1261
078adacd72a4 moving.py integrating Mohamed's comments refactored Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 607 diff changeset	1262 aggregationFunc can be any function that can be applied to a vector of speeds, including percentile:
078adacd72a4 moving.py integrating Mohamed's comments refactored Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 607 diff changeset	1263 aggregationFunc = lambda x: percentile(x, percentileFactor) # where percentileFactor is 85 for 85th percentile'''
501 c81cbd6953fb update to classify speed to remove data at both ends Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 484 diff changeset	1264 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	1265 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	1266 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	1267 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	1268 if aggregationFunc(speeds) >= threshold:
345 fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1269 self.setUserType(userType2Num['car'])
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1270 else:
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1271 self.setUserType(userType2Num['pedestrian'])
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1272
524 1dced8932b08 corrected bugs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 523 diff changeset	1273 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	1274 '''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	1275 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	1276 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	1277 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	1278
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	1279 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	1280 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	1281 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	1282 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	1283 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	1284 return x'''
626 35155ac2a294 corrected bugs, in particular to MovingObject.play Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 624 diff changeset	1285 if not hasattr(self, 'aggregatedSpeed'):
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	1286 self.aggregatedSpeed = aggregationFunc(self.getSpeeds())
581 10e8a9f2bd9f change for python 2.6 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 579 diff changeset	1287 userTypeProbabilities = {}
10e8a9f2bd9f change for python 2.6 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 579 diff changeset	1288 for userTypename in speedProbabilities:
10e8a9f2bd9f change for python 2.6 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 579 diff changeset	1289 userTypeProbabilities[userType2Num[userTypename]] = speedProbabilities[userTypename](self.aggregatedSpeed)
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	1290 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	1291 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	1292
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	1293 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	1294 '''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	1295
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	1296 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	1297 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	1298 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	1299 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	1300
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	1301 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	1302 '''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	1303 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	1304 from numpy import array
fd9641cbd24b added function to classify object at instant from SVM Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 517 diff changeset	1305 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	1306 if len(croppedImg) > 0: # != []
fd9641cbd24b added function to classify object at instant from SVM Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 517 diff changeset	1307 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	1308 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	1309 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	1310 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	1311 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	1312 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	1313 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	1314 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	1315 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	1316
524 1dced8932b08 corrected bugs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 523 diff changeset	1317 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	1318 '''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	1319 (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	1320
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	1321 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	1322 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	1323 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	1324 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	1325 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	1326 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	1327
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	1328 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	1329 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	1330 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	1331 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	1332 # compute P(Speed\|Class)
524 1dced8932b08 corrected bugs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 523 diff changeset	1333 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	1334 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	1335 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	1336 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	1337 # 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	1338 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	1339 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	1340 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	1341 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	1342 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	1343 # 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	1344 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	1345
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	1346 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	1347 '''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	1348 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	1349 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	1350
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	1351 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	1352 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	1353 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	1354 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	1355 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	1356 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	1357 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	1358 self.projectedPositions = obj.positions
523 ce4eaabacc26 modified internal implementation of user type for classifyUserTypeArea Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 522 diff changeset	1359 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	1360 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	1361 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	1362 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	1363 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	1364 # 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	1365 return possibleUserTypes
520 fd9641cbd24b added function to classify object at instant from SVM Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 517 diff changeset	1366
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1367 @staticmethod
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1368 def collisionCourseDotProduct(movingObject1, movingObject2, instant):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1369 '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	1370 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	1371 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	1372 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	1373
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1374 @staticmethod
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1375 def collisionCourseCosine(movingObject1, movingObject2, instant):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1376 '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	1377 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	1378 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	1379
587 cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	1380
cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	1381 ##################
588 c5406edbcf12 added loading ground truth annotations (ground truth) from polytrack format Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 587 diff changeset	1382 # Annotations
587 cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	1383 ##################
cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	1384
588 c5406edbcf12 added loading ground truth annotations (ground truth) from polytrack format Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 587 diff changeset	1385 class BBAnnotation(MovingObject):
590 0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1386 '''Class for a series of ground truth annotations using bounding boxes
588 c5406edbcf12 added loading ground truth annotations (ground truth) from polytrack format Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 587 diff changeset	1387 Its center is the center of the containing shape
590 0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1388
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1389 By default in image space
588 c5406edbcf12 added loading ground truth annotations (ground truth) from polytrack format Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 587 diff changeset	1390 '''
587 cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	1391
589 5800a87f11ae corrected one bug and changed attribute names Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 588 diff changeset	1392 def __init__(self, num = None, timeInterval = None, topLeftPositions = None, bottomRightPositions = None, userType = userType2Num['unknown']):
590 0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1393 super(BBAnnotation, self).__init__(num, timeInterval, userType = userType)
589 5800a87f11ae corrected one bug and changed attribute names Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 588 diff changeset	1394 self.topLeftPositions = topLeftPositions.getPositions()
5800a87f11ae corrected one bug and changed attribute names Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 588 diff changeset	1395 self.bottomRightPositions = bottomRightPositions.getPositions()
590 0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1396
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1397 def computeCentroidTrajectory(self, homography = None):
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1398 self.positions = self.topLeftPositions.add(self.bottomRightPositions).multiply(0.5)
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1399 if homography != None:
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1400 self.positions = self.positions.project(homography)
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1401
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1402 def matches(self, obj, instant, matchingDistance):
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1403 '''Indicates if the annotation matches obj (MovingObject)
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1404 with threshold matchingDistance
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1405 Returns distance if below matchingDistance, matchingDistance+1 otherwise
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1406 (returns an actual value, otherwise munkres does not terminate)'''
592 985a3021cff2 first match table implementation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 590 diff changeset	1407 d = Point.distanceNorm2(self.getPositionAtInstant(instant), obj.getPositionAtInstant(instant))
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1408 if d < matchingDistance:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1409 return d
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1410 else:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1411 return matchingDistance + 1
590 0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1412
595 17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	1413 def computeClearMOT(annotations, objects, matchingDistance, firstInstant, lastInstant, debug = False):
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1414 '''Computes the CLEAR MOT metrics
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1415
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1416 Reference:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1417 Keni, Bernardin, and Stiefelhagen Rainer. "Evaluating multiple object tracking performance: the CLEAR MOT metrics." EURASIP Journal on Image and Video Processing 2008 (2008)
590 0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1418
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1419 objects and annotations are supposed to in the same space
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1420 current implementation is BBAnnotations (bounding boxes)
592 985a3021cff2 first match table implementation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 590 diff changeset	1421 mathingDistance is threshold on matching between annotation and object
985a3021cff2 first match table implementation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 590 diff changeset	1422
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1423 TO: tracker output (objects)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1424 GT: ground truth (annotations)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1425
624 bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	1426 Output: returns motp, mota, mt, mme, fpt, gt
bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	1427 mt number of missed GT.frames (sum of the number of GT not detected in each frame)
bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	1428 mme number of mismatches
bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	1429 fpt number of false alarm.frames (tracker objects without match in each frame)
bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	1430 gt number of GT.frames
bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	1431
bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	1432 TODO: Should we use the distance as weights or just 1/0 if distance below matchingDistance?
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1433 (add argument useDistanceForWeights = False)'''
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1434 from munkres import Munkres
592 985a3021cff2 first match table implementation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 590 diff changeset	1435
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1436 munk = Munkres()
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1437 dist = 0. # total distance between GT and TO
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1438 ct = 0 # number of associations between GT and tracker output in each frame
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1439 gt = 0 # number of GT.frames
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1440 mt = 0 # number of missed GT.frames (sum of the number of GT not detected in each frame)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1441 fpt = 0 # number of false alarm.frames (tracker objects without match in each frame)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1442 mme = 0 # number of mismatches
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1443 matches = {} # match[i] is the tracker track associated with GT i (using object references)
592 985a3021cff2 first match table implementation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 590 diff changeset	1444 for t in xrange(firstInstant, lastInstant+1):
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1445 previousMatches = matches.copy()
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1446 # go through currently matched GT-TO and check if they are still matched withing matchingDistance
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1447 toDelete = []
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1448 for a in matches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1449 if a.existsAtInstant(t) and matches[a].existsAtInstant(t):
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1450 d = a.matches(matches[a], t, matchingDistance)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1451 if d < matchingDistance:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1452 dist += d
592 985a3021cff2 first match table implementation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 590 diff changeset	1453 else:
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1454 toDelete.append(a)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1455 else:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1456 toDelete.append(a)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1457 for a in toDelete:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1458 del matches[a]
593 e2a873e08568 non-working clear mot metrics Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 592 diff changeset	1459
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1460 # match all unmatched GT-TO
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1461 matchedGTs = matches.keys()
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1462 matchedTOs = matches.values()
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1463 costs = []
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1464 unmatchedGTs = [a for a in annotations if a.existsAtInstant(t) and a not in matchedGTs]
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1465 unmatchedTOs = [o for o in objects if o.existsAtInstant(t) and o not in matchedTOs]
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1466 nGTs = len(matchedGTs)+len(unmatchedGTs)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1467 nTOs = len(matchedTOs)+len(unmatchedTOs)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1468 if len(unmatchedTOs) > 0:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1469 for a in unmatchedGTs:
595 17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	1470 costs.append([a.matches(o, t, matchingDistance) for o in unmatchedTOs])
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1471 if len(costs) > 0:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1472 newMatches = munk.compute(costs)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1473 for k,v in newMatches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1474 if costs[k][v] < matchingDistance:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1475 matches[unmatchedGTs[k]]=unmatchedTOs[v]
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1476 dist += costs[k][v]
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1477 if debug:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1478 print('{} '.format(t)+', '.join(['{} {}'.format(k.getNum(), v.getNum()) for k,v in matches.iteritems()]))
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1479
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1480 # compute metrics elements
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1481 ct += len(matches)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1482 mt += nGTs-len(matches)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1483 fpt += nTOs-len(matches)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1484 gt += nGTs
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1485 # compute mismatches
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1486 # for gt that do not appear in both frames, check if the corresponding to was matched to another gt in previous/next frame
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1487 mismatches = []
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1488 for a in matches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1489 if a in previousMatches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1490 if matches[a] != previousMatches[a]:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1491 mismatches.append(a)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1492 elif matches[a] in previousMatches.values():
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1493 mismatches.append(matches[a])
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1494 for a in previousMatches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1495 if a not in matches and previousMatches[a] in matches.values():
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1496 mismatches.append(previousMatches[a])
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1497 if debug:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1498 for mm in set(mismatches):
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1499 print type(mm), mm.getNum()
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1500 # some object mismatches may appear twice
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1501 mme += len(set(mismatches))
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1502
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1503 if ct > 0:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1504 motp = dist/ct
593 e2a873e08568 non-working clear mot metrics Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 592 diff changeset	1505 else:
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1506 motp = None
595 17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	1507 if gt > 0:
17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	1508 mota = 1.-float(mt+fpt+mme)/gt
17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	1509 else:
17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	1510 mota = None
17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	1511 return motp, mota, mt, mme, fpt, gt
593 e2a873e08568 non-working clear mot metrics Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 592 diff changeset	1512
e2a873e08568 non-working clear mot metrics Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 592 diff changeset	1513
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1514 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	1515 '''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	1516 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	1517 figure()
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1518 for obj in objects:
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	1519 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	1520 axis('equal')
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1521
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1522
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1523 if __name__ == "__main__":
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1524 import doctest
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1525 import unittest
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1526 suite = doctest.DocFileSuite('tests/moving.txt')
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1527 #suite = doctest.DocTestSuite()
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1528 unittest.TextTestRunner().run(suite)
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1529 #doctest.testmod()
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1530 #doctest.testfile("example.txt")

Mercurial Hosting > traffic-intelligence

annotate python/moving.py @ 630:69a98f84f3eb