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

annotate python/moving.py @ 620:aee4cbac9e0e

corrected bug to plot trajectories on world image (the norm on image coordinates seems to have changed in matplotlib)

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Tue, 09 Dec 2014 17:27:14 -0500
parents	17b02c8054d0
children	582508610572

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

Mercurial Hosting > traffic-intelligence

annotate python/moving.py @ 620:aee4cbac9e0e