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

annotate python/moving.py @ 781:7c38250ddfc7 dev

updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example)

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Thu, 11 Feb 2016 11:56:50 -0500
parents	1b22d81ef5ff
children	3aa6102ccc12

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
664 455f9b93819c added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 663 diff changeset	4 import utils, cvutils
665 15e244d2a1b5 corrected bug with circular import for VideoFilenameAddable, moved to base module Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 664 diff changeset	5 from base import VideoFilenameAddable
0 aed8eb63cdde initial commit with non-functional python code for NGSIM Nicolas Saunier <nico@confins.net> parents: diff changeset	6
672 5473b7460375 moved and rationalized imports in modules Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 666 diff changeset	7 from math import sqrt, atan2, cos, sin
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	8 from numpy import median, array, zeros, hypot, NaN, std, floor, float32
672 5473b7460375 moved and rationalized imports in modules Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 666 diff changeset	9 from matplotlib.pyplot import plot
5473b7460375 moved and rationalized imports in modules Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 666 diff changeset	10 from scipy.stats import scoreatpercentile
5473b7460375 moved and rationalized imports in modules Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 666 diff changeset	11 from scipy.spatial.distance import cdist
16 9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	12
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	13 try:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	14 from shapely.geometry import Polygon, Point as shapelyPoint
781 7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	15 from shapely.prepared import prep, PreparedGeometry
372 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 = True
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	17 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	18 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	19 shapelyAvailable = False
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	20
0 aed8eb63cdde initial commit with non-functional python code for NGSIM Nicolas Saunier <nico@confins.net> parents: diff changeset	21
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	22 class Interval(object):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	23 '''Generic interval: a subset of real numbers (not iterable)'''
26 54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	24 def __init__(self, first=0, last=-1, revert = False):
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	25 if revert and last<first:
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	26 self.first=last
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	27 self.last=first
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	28 else:
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	29 self.first=first
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	30 self.last=last
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	31
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	32 def __str__(self):
104 13187af8622d finally added the representation of intervals Nicolas Saunier <nico@confins.net> parents: 98 diff changeset	33 return '[{0}, {1}]'.format(self.first, self.last)
13187af8622d finally added the representation of intervals Nicolas Saunier <nico@confins.net> parents: 98 diff changeset	34
13187af8622d finally added the representation of intervals Nicolas Saunier <nico@confins.net> parents: 98 diff changeset	35 def __repr__(self):
13187af8622d finally added the representation of intervals Nicolas Saunier <nico@confins.net> parents: 98 diff changeset	36 return self.__str__()
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	37
776 84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	38 def __eq__(self, other):
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	39 return ((self.first == other.first) and (self.last == other.last)) or ((self.first == other.last) and (self.last == other.first))
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	40
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	41 def empty(self):
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	42 return self.first > self.last
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	43
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	44 def center(self):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	45 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	46
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	47 def length(self):
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	48 '''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	49 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	50
96 9928c2fa72cc added equal method to intervals Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 93 diff changeset	51 def equal(self, i2):
9928c2fa72cc added equal method to intervals Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 93 diff changeset	52 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	53
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	54 def getList(self):
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	55 return [self.first, self.last]
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	56
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	57 def contains(self, instant):
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	58 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	59
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	60 def inside(self, interval2):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	61 '''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	62 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	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 union(cls, interval1, interval2):
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	66 '''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	67 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	68
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	69 @classmethod
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	70 def intersection(cls, interval1, interval2):
104 13187af8622d finally added the representation of intervals Nicolas Saunier <nico@confins.net> parents: 98 diff changeset	71 '''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	72 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	73
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	74 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	75 if not Interval.intersection(self, interval2).empty():
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	76 return 0
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	77 elif self.first > interval2.last:
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	78 return self.first - interval2.last
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	79 elif self.last < interval2.first:
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	80 return interval2.first - self.last
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	81 else:
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	82 return None
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	83
727 c6d4ea05a2d0 adding ability to deal with multivariate indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 726 diff changeset	84 @classmethod
c6d4ea05a2d0 adding ability to deal with multivariate indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 726 diff changeset	85 def unionIntervals(cls, intervals):
c6d4ea05a2d0 adding ability to deal with multivariate indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 726 diff changeset	86 'returns the smallest interval containing all intervals'
c6d4ea05a2d0 adding ability to deal with multivariate indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 726 diff changeset	87 inter = cls(intervals[0].first, intervals[0].last)
c6d4ea05a2d0 adding ability to deal with multivariate indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 726 diff changeset	88 for i in intervals[1:]:
c6d4ea05a2d0 adding ability to deal with multivariate indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 726 diff changeset	89 inter = cls.union(inter, i)
c6d4ea05a2d0 adding ability to deal with multivariate indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 726 diff changeset	90 return inter
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	91
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	92
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	93 class TimeInterval(Interval):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	94 '''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	95
e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	96 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	97 It may be modified directly by setting first and last'''
26 54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	98
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	99 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	100 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	101
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	102 @staticmethod
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	103 def fromInterval(inter):
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	104 return TimeInterval(inter.first, inter.last)
26 54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	105
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	106 def __getitem__(self, i):
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	107 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	108 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	109 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	110 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	111 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	112 #elif isinstance( key, slice ):
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	113
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	114 def __iter__(self):
107 916678481896 corrected bug for TimeInterval interation and added corresponding test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 105 diff changeset	115 self.iterInstantNum = -1
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	116 return self
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	117
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	118 def next(self):
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	119 if self.iterInstantNum >= self.length()-1:
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	120 raise StopIteration
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	121 else:
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	122 self.iterInstantNum += 1
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	123 return self[self.iterInstantNum]
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	124
26 54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	125 def length(self):
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	126 '''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	127 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	128
688 f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	129 def __len__(self):
f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	130 return self.length()
f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	131
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	132 # class BoundingPolygon:
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	133 # '''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	134 # with methods to create intersection, unions...
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	135 # '''
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	136 # We will use the polygon class of Shapely
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	137
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	138 class STObject(object):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	139 '''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	140 (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	141
e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	142 It may not mean that the object is defined
6 597d61c1eebe minor doc update Nicolas Saunier <nico@confins.net> parents: 2 diff changeset	143 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	144
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	145 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	146 self.num = num
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	147 self.timeInterval = timeInterval
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	148 self.boundingPolygon = boundingPolygon
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	149
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	150 def empty(self):
688 f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	151 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	152
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	153 def getNum(self):
211 ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	154 return self.num
ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	155
688 f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	156 def __len__(self):
f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	157 return self.timeInterval.length()
f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	158
f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	159 def length(self):
f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	160 return self.timeInterval.length()
f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	161
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	162 def getFirstInstant(self):
40 9f16aee24b7e corrected bug for first and last of TimeInterval Nicolas Saunier <nico@confins.net> parents: 39 diff changeset	163 return self.timeInterval.first
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	164
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	165 def getLastInstant(self):
40 9f16aee24b7e corrected bug for first and last of TimeInterval Nicolas Saunier <nico@confins.net> parents: 39 diff changeset	166 return self.timeInterval.last
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	167
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	168 def getTimeInterval(self):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	169 return self.timeInterval
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	170
108 6efe470ea5e5 added test existsAtInstant to STObject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 107 diff changeset	171 def existsAtInstant(self, t):
6efe470ea5e5 added test existsAtInstant to STObject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 107 diff changeset	172 return self.timeInterval.contains(t)
6efe470ea5e5 added test existsAtInstant to STObject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 107 diff changeset	173
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	174 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	175 return TimeInterval.intersection(self.getTimeInterval(), obj2.getTimeInterval())
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	176
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	177 class Point(object):
25 28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	178 def __init__(self, x, y):
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	179 self.x = x
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	180 self.y = y
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	181
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	182 def __str__(self):
675 ab3fdff42624 corrected old format for Point.__str__ Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 674 diff changeset	183 return '({:f},{:f})'.format(self.x,self.y)
25 28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	184
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	185 def __repr__(self):
98 b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	186 return self.__str__()
25 28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	187
776 84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	188 def __eq__(self, other):
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	189 return (self.x == other.x) and (self.y == other.y)
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	190
184 d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	191 def __add__(self, other):
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	192 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	193
25 28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	194 def __sub__(self, other):
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	195 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	196
451 cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	197 def __neg__(self):
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	198 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	199
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	200 def __getitem__(self, i):
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	201 if i == 0:
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	202 return self.x
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	203 elif i == 1:
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	204 return self.y
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	205 else:
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	206 raise IndexError()
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	207
690 463150a8e129 minor updates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 688 diff changeset	208 def orthogonal(self, clockwise = True):
463150a8e129 minor updates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 688 diff changeset	209 'Returns the orthogonal vector'
463150a8e129 minor updates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 688 diff changeset	210 if clockwise:
463150a8e129 minor updates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 688 diff changeset	211 return Point(self.y, -self.x)
463150a8e129 minor updates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 688 diff changeset	212 else:
463150a8e129 minor updates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 688 diff changeset	213 return Point(-self.y, self.x)
451 cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	214
105 9844c69d8fa2 added multiply method to Point Nicolas Saunier <nico@confins.net> parents: 104 diff changeset	215 def multiply(self, alpha):
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	216 'Warning, returns a new Point'
105 9844c69d8fa2 added multiply method to Point Nicolas Saunier <nico@confins.net> parents: 104 diff changeset	217 return Point(self.xalpha, self.yalpha)
9844c69d8fa2 added multiply method to Point Nicolas Saunier <nico@confins.net> parents: 104 diff changeset	218
623 ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	219 def divide(self, alpha):
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	220 'Warning, returns a new Point'
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	221 return Point(self.x/alpha, self.y/alpha)
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	222
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	223 def plot(self, options = 'o', **kwargs):
211 ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	224 plot([self.x], [self.y], options, **kwargs)
41 eb78c6edc0c8 added drawing for Point Nicolas Saunier <nico@confins.net> parents: 40 diff changeset	225
771 bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	226 @staticmethod
bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	227 def plotSegment(p1, p2, options = 'o', **kwargs):
bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	228 plot([p1.x, p2.x], [p1.y, p2.y], options, **kwargs)
bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	229
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	230 def norm2Squared(self):
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	231 '''2-norm distance (Euclidean distance)'''
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	232 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	233
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	234 def norm2(self):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	235 '''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	236 return sqrt(self.norm2Squared())
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	237
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	238 def norm1(self):
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	239 return abs(self.x)+abs(self.y)
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	240
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	241 def normMax(self):
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	242 return max(abs(self.x),abs(self.y))
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	243
64 c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	244 def aslist(self):
c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	245 return [self.x, self.y]
c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	246
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	247 def astuple(self):
e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	248 return (self.x, self.y)
e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	249
223 c31722fcc9de minor modifications for integer drawing in OpenCV Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 214 diff changeset	250 def asint(self):
c31722fcc9de minor modifications for integer drawing in OpenCV Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 214 diff changeset	251 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	252
372 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 shapelyAvailable:
349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	254 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	255 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	256
98 b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	257 def project(self, homography):
b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	258 projected = cvutils.projectArray(homography, array([[self.x], [self.y]]))
b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	259 return Point(projected[0], projected[1])
b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	260
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	261 def inPolygon(self, polygon):
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	262 '''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	263 (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	264
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	265 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	266
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	267 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	268
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	269 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	270 counter = 0;
5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	271
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	272 p1 = polygon[0,:];
79 5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	273 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	274 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	275 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	276 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	277 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	278 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	279 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	280 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	281 counter+=1;
5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	282 p1=p2
5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	283 return (counter%2 == 1);
5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	284
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	285 @staticmethod
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	286 def fromList(p):
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	287 return Point(p[0], p[1])
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	288
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	289 @staticmethod
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	290 def dot(p1, p2):
89 f88a19695bba added inner product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 83 diff changeset	291 'Scalar product'
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	292 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	293
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	294 @staticmethod
90 f84293ad4611 renamed inner to cross product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 89 diff changeset	295 def cross(p1, p2):
f84293ad4611 renamed inner to cross product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 89 diff changeset	296 'Cross product'
89 f88a19695bba added inner product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 83 diff changeset	297 return p1.xp2.y-p1.yp2.x
f88a19695bba added inner product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 83 diff changeset	298
f88a19695bba added inner product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 83 diff changeset	299 @staticmethod
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	300 def cosine(p1, p2):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	301 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	302
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	303 @staticmethod
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	304 def distanceNorm2(p1, p2):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	305 return (p1-p2).norm2()
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	306
64 c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	307 @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	308 def plotAll(points, **kwargs):
64 c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	309 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	310 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	311
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	312 def similarOrientation(self, refDirection, cosineThreshold):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	313 '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	314 return Point.cosine(self, refDirection) >= cosineThreshold
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	315
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	316 @staticmethod
484 6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	317 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	318 '''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	319 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	320 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	321 and the circle of radius collisionThreshold around the other road user'''
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	322 dv = v1-v2
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	323 dp = p1-p2
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	324 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	325 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	326 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	327
504 a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	328 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	329 if delta >= 0:
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	330 deltaRoot = sqrt(delta)
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	331 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	332 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	333 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	334 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	335 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	336 ttc = ttc1
f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 527 diff changeset	337 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	338 ttc = ttc2
f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 527 diff changeset	339 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	340 ttc = None
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	341 else:
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	342 ttc = None
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	343 return ttc
dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	344
583 6ebfb43e938e added midpoint function (from laurent gauthier) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 582 diff changeset	345 @staticmethod
6ebfb43e938e added midpoint function (from laurent gauthier) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 582 diff changeset	346 def midPoint(p1, p2):
6ebfb43e938e added midpoint function (from laurent gauthier) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 582 diff changeset	347 '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	348 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	349
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	350 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	351 def pointsInPolygon(points, polygon):
781 7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	352 '''Optimized tests of a series of points within (Shapely) polygon (not prepared)'''
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	353 if type(polygon) == PreparedGeometry:
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	354 prepared_polygon = polygon
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	355 else:
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	356 prepared_polygon = prep(polygon)
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	357 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	358
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	359 # Functions for coordinate transformation
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	360 # 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	361 def subsec_spline_dist(splines):
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	362 ''' 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	363
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	364 Output:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	365 =======
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	366 ss_spline_d[spline #][mode][station]
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 where:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	369 mode=0: incremental distance
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	370 mode=1: cumulative distance
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	371 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	372 '''
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	373 ss_spline_d = []
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	374 #Prepare subsegment distances
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	375 for spline in range(len(splines)):
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	376 ss_spline_d[spline]=[]#.append([[],[],[]])
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	377 ss_spline_d[spline].append(zeros(len(splines[spline])-1)) #Incremental distance
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	378 ss_spline_d[spline].append(zeros(len(splines[spline])-1)) #Cumulative distance
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	379 ss_spline_d[spline].append(zeros(len(splines[spline]))) #Cumulative distance with trailing distance
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	380 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	381 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	382 break
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	383 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	384 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	385 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	386
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	387 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	388
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	389 return ss_spline_d
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	390
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	391 def prepareSplines(splines):
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	392 'Approximates slope singularity by giving some slope roundoff; account for roundoff error'
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	393 for spline in splines:
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	394 p1 = spline[0]
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	395 for i in xrange(len(spline)-1):
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	396 p2 = spline[i+1]
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	397 if(round(p1.x, 10) == round(p2.x, 10)):
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	398 p2.x += 0.0000000001
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	399 if(round(p1.y, 10) == round(p2.y, 10)):
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	400 p2.y += 0.0000000001
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	401 p1 = p2
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	402
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	403 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	404 ''' 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	405 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	406 '''
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	407 if(p0x == p1x and p0y == p1y):
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	408 return None
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	409 try:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	410 #Approximate slope singularity by giving some slope roundoff; account for roundoff error
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	411 # if(round(p0x, 10) == round(p1x, 10)):
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	412 # p1x += 0.0000000001
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	413 # if(round(p0y, 10) == round(p1y, 10)):
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	414 # p1y += 0.0000000001
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	415 #make the calculation
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	416 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	417 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	418 except ZeroDivisionError:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	419 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	420 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	421 import pdb; pdb.set_trace()
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	422 return Point(X,Y)
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	423
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	424 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5):
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	425 ''' Snap a point p to it's nearest subsegment of it's nearest spline (from the list splines).
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	426 A spline is a list of points (class Point), most likely a trajectory.
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	427
659 784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	428 Output:
784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	429 =======
784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	430 [spline index,
784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	431 subsegment leading point index,
784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	432 snapped point,
784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	433 subsegment distance,
784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	434 spline distance,
784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	435 orthogonal point offset]
784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	436
784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	437 or None
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	438 '''
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	439 minOffsetY = float('inf')
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	440 #For each spline
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	441 for splineIdx in range(len(splines)):
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	442 #For each spline point index
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	443 for spline_p in range(len(splines[splineIdx])-1):
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	444 #Get closest point on spline
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	445 closestPoint = ppldb2p(p.x,p.y,splines[splineIdx][spline_p][0],splines[splineIdx][spline_p][1],splines[splineIdx][spline_p+1][0],splines[splineIdx][spline_p+1][1])
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	446 if closestPoint is None:
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	447 print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(splineIdx, spline_p))
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	448 return None
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	449 # check if the projected point is in between the current segment of the alignment bounds
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	450 if utils.inBetween(splines[splineIdx][spline_p][0], splines[splineIdx][spline_p+1][0], closestPoint.x) and utils.inBetween(splines[splineIdx][spline_p][1], splines[splineIdx][spline_p+1][1], closestPoint.y):
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	451 offsetY = Point.distanceNorm2(closestPoint, p)
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	452 if offsetY < minOffsetY:
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	453 minOffsetY = offsetY
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	454 snappedSplineIdx = splineIdx
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	455 snappedSplineLeadingPoint = spline_p
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	456 snappedPoint = Point(closestPoint.x, closestPoint.y)
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	457 #Jump loop if significantly close
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	458 if offsetY < goodEnoughSplineDistance:
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	459 break
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	460
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	461 #Get sub-segment distance
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	462 if minOffsetY != float('inf'):
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	463 subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSplineIdx][snappedSplineLeadingPoint])
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	464 #Get cumulative alignment distance (total segment distance)
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	465 splineDistanceS = splines[snappedSplineIdx].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	466 orthogonalSplineVector = (splines[snappedSplineIdx][snappedSplineLeadingPoint+1]-splines[snappedSplineIdx][snappedSplineLeadingPoint]).orthogonal()
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	467 offsetVector = p-snappedPoint
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	468 if Point.dot(orthogonalSplineVector, offsetVector) < 0:
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	469 minOffsetY = -minOffsetY
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	470 return [snappedSplineIdx, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY]
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	471 else:
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	472 print('Offset for point {} is infinite (check with prepareSplines if some spline segments are aligned with axes)'.format(p))
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	473 return None
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	474
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	475 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	476 ''' Find X,Y coordinate from S,Y data.
568 538fb47b3007 minor modification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	477 if mode = 0 : return Snapped X,Y
538fb47b3007 minor modification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	478 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	479 '''
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	480
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	481 #(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	482 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	483
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	484 #Find subsegment
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	485 snapped_x = None
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	486 snapped_y = None
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	487 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	488 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	489 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	490 #Get normal vector and then snap
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	491 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	492 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	493 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	494 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	495 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	496 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	497 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	498
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	499 #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	500 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	501 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	502 break
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	503
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	504 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	505 if(not snapped_x):
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	506 snapped_x = splines[splineNum][-1][0]
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	507 snapped_y = splines[splineNum][-1][1]
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	508 return [snapped_x,snapped_y]
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	509 else:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	510 return [real_x,real_y]
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	511
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	512
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	513 class NormAngle(object):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	514 '''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	515
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	516 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	517 self.norm = norm
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	518 self.angle = angle
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	519
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	520 @staticmethod
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	521 def fromPoint(p):
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	522 norm = p.norm2()
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	523 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	524 angle = atan2(p.y, p.x)
339 9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	525 else:
9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	526 angle = 0.
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	527 return NormAngle(norm, angle)
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	528
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	529 def __add__(self, other):
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	530 '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	531 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	532
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	533 def getPoint(self):
255 13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	534 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	535
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	536
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	537 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	538 '''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	539 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	540
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	541 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	542 '''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	543 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	544 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	545 predictedSpeedTheta = speedOrientation+control
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	546 if maxSpeed:
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	547 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	548 predictedPosition = position+predictedSpeedTheta.getPoint()
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	549 return predictedPosition, predictedSpeedTheta
245 bd8ab323c198 corrected issue with predictPosiont static method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 244 diff changeset	550
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	551
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	552 class FlowVector(object):
184 d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	553 '''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	554 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	555 def __init__(self, position, velocity):
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	556 '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	557 self.position = position
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	558 self.velocity = velocity
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	559
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	560 def __add__(self, other):
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	561 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	562
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	563 def multiply(self, alpha):
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	564 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	565
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	566 def plot(self, options = '', **kwargs):
211 ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	567 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	568 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	569
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	570 @staticmethod
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	571 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	572 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	573
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	574 def intersection(p1, p2, p3, p4):
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	575 ''' 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	576 http://paulbourke.net/geometry/pointlineplane/'''
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	577 dp12 = p2-p1
571 a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	578 dp34 = p4-p3
a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	579 #det = (p4.y-p3.y)(p2.x-p1.x)-(p4.x-p3.x)(p2.y-p1.y)
674 01b89182891a corrected bug for intersection of lines (thanks to Paul for finding) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 672 diff changeset	580 det = float(dp34.ydp12.x-dp34.xdp12.y)
01b89182891a corrected bug for intersection of lines (thanks to Paul for finding) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 672 diff changeset	581 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	582 return None
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	583 else:
571 a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	584 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	585 return p1+dp12.multiply(ua)
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	586
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	587 # def intersection(p1, p2, dp1, dp2):
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	588 # '''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	589 # 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	590 # from numpy import matrix
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	591 # from numpy.linalg import linalg
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	592 # A = matrix([[dp1.y, -dp1.x],
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	593 # [dp2.y, -dp2.x]])
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	594 # 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	595 # [dp2.yp2.x-dp2.xp2.y]])
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	596
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	597 # if linalg.det(A) == 0:
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	598 # return None
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	599 # else:
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	600 # intersection = linalg.solve(A,B)
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	601 # 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	602
152 74b1fc68d4df re-organized code to avoid cyclic python module dependencies Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 148 diff changeset	603 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	604 '''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	605
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	606 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	607 return None
74b1fc68d4df re-organized code to avoid cyclic python module dependencies Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 148 diff changeset	608 else:
571 a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	609 inter = intersection(p1, p2, p3, p4)
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	610 if (inter is not None
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	611 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	612 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	613 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	614 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	615 return inter
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	616 else:
258 d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	617 return None
d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	618
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	619 def segmentLineIntersection(p1, p2, p3, p4):
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	620 '''Indicates if the line going through p1 and p2 intersects inside p3, p4'''
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	621 inter = intersection(p1, p2, p3, p4)
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	622 if inter is not None and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p3.y, p4.y, inter.y):
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	623 return inter
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	624 else:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	625 return None
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	626
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	627
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	628 class Trajectory(object):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	629 '''Class for trajectories: temporal sequence of positions
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	630
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	631 The class is iterable'''
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	632
79 5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	633 def __init__(self, positions=None):
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	634 if positions is not None:
113 606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	635 self.positions = positions
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	636 else:
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	637 self.positions = [[],[]]
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	638
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	639 @staticmethod
555 f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	640 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	641 t = Trajectory()
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	642 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	643 t.addPosition(p0)
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	644 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	645 p0 += v
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	646 t.addPosition(p0)
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	647 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	648
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	649 @staticmethod
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	650 def load(line1, line2):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	651 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	652 [float(n) for n in line2.split(' ')]])
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	653
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	654 @staticmethod
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	655 def fromPointList(points):
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	656 t = Trajectory()
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	657 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	658 for p in points:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	659 t.addPositionXY(p[0],p[1])
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	660 else:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	661 for p in points:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	662 t.addPosition(p)
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	663 return t
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	664
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	665 def __len__(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	666 return len(self.positions[0])
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	667
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	668 def length(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	669 return self.__len__()
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	670
582 7e1ae4d97f1a corrected bug for curvilinear trajectory with only one position and differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 581 diff changeset	671 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	672 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	673
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	674 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	675 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	676 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	677 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	678 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	679 #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	680
16 9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	681 def __str__(self):
39 e47168f6b694 corrected printing bug Nicolas Saunier <nico@confins.net> parents: 38 diff changeset	682 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	683
69 cc192d0450b3 added full support for two implementations of indicators, with tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 68 diff changeset	684 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	685 return self.__str__()
23 5f2921ad4f7e made Trajectory indexable and timeinterval iterable Nicolas Saunier <nico@confins.net> parents: 22 diff changeset	686
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	687 def __iter__(self):
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	688 self.iterInstantNum = 0
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	689 return self
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	690
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	691 def next(self):
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	692 if self.iterInstantNum >= self.length():
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	693 raise StopIteration
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	694 else:
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	695 self.iterInstantNum += 1
23 5f2921ad4f7e made Trajectory indexable and timeinterval iterable Nicolas Saunier <nico@confins.net> parents: 22 diff changeset	696 return self[self.iterInstantNum-1]
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	697
776 84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	698 def __eq__(self, other):
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	699 if self.length() == other.length():
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	700 result = True
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	701 for p, po in zip(self, other):
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	702 result = result and (p == po)
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	703 return result
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	704 else:
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	705 return False
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	706
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	707 def setPositionXY(self, i, x, y):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	708 if i < self.__len__():
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	709 self.positions[0][i] = x
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	710 self.positions[1][i] = y
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	711
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	712 def setPosition(self, i, p):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	713 self.setPositionXY(i, p.x, p.y)
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	714
71 45e958ccd9bd added new addPosition method to Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 69 diff changeset	715 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	716 self.positions[0].append(x)
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	717 self.positions[1].append(y)
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	718
71 45e958ccd9bd added new addPosition method to Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 69 diff changeset	719 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	720 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	721
343 74e437ab5f11 first version of indicator loading code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 339 diff changeset	722 def duplicateLastPosition(self):
74e437ab5f11 first version of indicator loading code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 339 diff changeset	723 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	724 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	725
182 d3f6de6c3918 added drawing functialities, in particular on aerial image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 152 diff changeset	726 @staticmethod
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	727 def _plot(positions, options = '', withOrigin = False, lastCoordinate = None, timeStep = 1, **kwargs):
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	728 if lastCoordinate is None:
270 05c9b0cb8202 updates for drawing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 268 diff changeset	729 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	730 elif 0 <= lastCoordinate <= len(positions[0]):
270 05c9b0cb8202 updates for drawing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 268 diff changeset	731 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	732 if withOrigin:
211 ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	733 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	734
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	735 def project(self, homography):
622 dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	736 return Trajectory(cvutils.projectTrajectory(homography, self.positions).tolist())
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	737
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	738 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	739 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	740
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	741 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	742 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	743
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	744 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	745 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	746 [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	747 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	748
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	749 def getXCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	750 return self.positions[0]
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	751
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	752 def getYCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	753 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	754
a5ef9e40688e makes use of matplotlib function to test if point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 91 diff changeset	755 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	756 return array(self.positions)
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	757
14 e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	758 def xBounds(self):
e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	759 # 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	760 return Interval(min(self.getXCoordinates()), max(self.getXCoordinates()))
14 e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	761
e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	762 def yBounds(self):
e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	763 # 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	764 return Interval(min(self.getYCoordinates()), max(self.getYCoordinates()))
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	765
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	766 def add(self, traj2):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	767 '''Returns a new trajectory of the same length'''
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	768 if self.length() != traj2.length():
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	769 print 'Trajectories of different lengths'
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	770 return None
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	771 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	772 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	773 [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	774
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	775 def subtract(self, traj2):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	776 '''Returns a new trajectory of the same length'''
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	777 if self.length() != traj2.length():
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	778 print 'Trajectories of different lengths'
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	779 return None
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	780 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	781 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	782 [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	783
590 0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	784 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	785 '''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	786 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	787 [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	788
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	789 def differentiate(self, doubleLastPosition = False):
339 9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	790 diff = Trajectory()
9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	791 for i in xrange(1, self.length()):
9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	792 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	793 if doubleLastPosition:
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	794 diff.addPosition(diff[-1])
339 9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	795 return diff
9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	796
761 15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	797 def differentiateSG(self, window_length, polyorder, deriv=0, delta=1.0, axis=-1, mode='interp', cval=0.0, removeBothEnds = 2):
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	798 '''Differentiates the trajectory using the Savitsky Golay filter
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	799
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	800 window_length : The length of the filter window (i.e. the number of coefficients). window_length must be a positive odd integer.
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	801 polyorder : The order of the polynomial used to fit the samples. polyorder must be less than window_length.
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	802 deriv : The order of the derivative to compute. This must be a nonnegative integer. The default is 0, which means to filter the data without differentiating.
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	803 delta : The spacing of the samples to which the filter will be applied. This is only used if deriv > 0. Default is 1.0.
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	804 axis : The axis of the array x along which the filter is to be applied. Default is -1.
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	805 mode : Must be mirror, constant, nearest, wrap or interp. This determines the type of extension to use for the padded signal to which the filter is applied. When mode is constant, the padding value is given by cval. See the Notes for more details on mirror, constant, wrap, and nearest. When the interp mode is selected (the default), no extension is used. Instead, a degree polyorder polynomial is fit to the last window_length values of the edges, and this polynomial is used to evaluate the last window_length // 2 output values.
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	806 cval : Value to fill past the edges of the input if mode is constant. Default is 0.0.'''
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	807 from scipy.signal import savgol_filter
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	808
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	809 if removeBothEnds >=1:
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	810 pos = [self.positions[0][removeBothEnds:-removeBothEnds],
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	811 self.positions[1][removeBothEnds:-removeBothEnds]]
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	812 else:
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	813 pos = self.positions
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	814 filtered = savgol_filter(pos, window_length, polyorder, deriv, delta, axis, mode, cval)
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	815 return Trajectory(filtered)
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	816
16 9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	817 def norm(self):
9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	818 '''Returns the list of the norms at each instant'''
9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	819 # def add(x, y): return x+y
9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	820 # 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	821 # return sqrt(sq)
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	822 return hypot(self.positions[0], self.positions[1])
14 e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	823
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	824 # def cumulatedDisplacement(self):
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	825 # '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	826 # displacement = 0
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	827 # for i in xrange(self.length()-1):
d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	828 # 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	829 # return displacement
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	830
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	831 def computeCumulativeDistances(self):
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	832 '''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	833 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	834 self.distances = []
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	835 self.cumulativeDistances = [0.]
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	836 p1 = self[0]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	837 cumulativeDistance = 0.
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	838 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	839 p2 = self[i+1]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	840 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	841 cumulativeDistance += self.distances[-1]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	842 self.cumulativeDistances.append(cumulativeDistance)
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	843 p1 = p2
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	844
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	845 def getDistance(self,i):
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	846 '''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	847 if i < self.length()-1:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	848 return self.distances[i]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	849 else:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	850 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	851
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	852 def getCumulativeDistance(self, i):
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	853 '''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	854 if i < self.length():
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	855 return self.cumulativeDistances[i]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	856 else:
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	857 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	858
762 d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	859 def getMaxDistance(self, metric):
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	860 'Returns the maximum distance between points in the trajectory'
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	861 positions = self.getPositions().asArray().T
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	862 return cdist(positions, positions, metric = metric).max()
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	863
381 387cc0142211 script to replay event annotations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 379 diff changeset	864 def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5):
387cc0142211 script to replay event annotations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 379 diff changeset	865 '''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	866 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	867 count = 0
383 0ce2210790b1 fixed stupid naming bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 381 diff changeset	868 lengthThreshold = float(self.length())*minProportion
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	869 for p in self:
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	870 if p.similarOrientation(refDirection, cosineThreshold):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	871 count += 1
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	872 return count >= lengthThreshold
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	873
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	874 def wiggliness(self):
762 d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	875 straightDistance = Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1))
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	876 if straightDistance > 0:
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	877 return self.getCumulativeDistance(self.length()-1)/float(straightDistance)
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	878 else:
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	879 return None
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	880
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	881 def getIntersections(self, p1, p2):
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	882 '''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	883 intersects with the segment of extremities p1 and p2
771 bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	884 Returns an empty list if there is no crossing'''
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	885 indices = []
631 2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	886 intersections = []
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	887
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	888 for i in xrange(self.length()-1):
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	889 q1=self.__getitem__(i)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	890 q2=self.__getitem__(i+1)
771 bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	891 p = segmentIntersection(q1, q2, p1, p2)
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	892 if p is not None:
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	893 if q1.x != q2.x:
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	894 ratio = (p.x-q1.x)/(q2.x-q1.x)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	895 elif q1.y != q2.y:
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	896 ratio = (p.y-q1.y)/(q2.y-q1.y)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	897 else:
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	898 ratio = 0
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	899 indices.append(i+ratio)
631 2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	900 intersections.append(p)
771 bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	901 return indices, intersections
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	902
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	903 def getLineIntersections(self, p1, p2):
631 2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	904 '''Returns a list of the indices at which the trajectory
771 bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	905 intersects with the line going through p1 and p2
bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	906 Returns an empty list if there is no crossing'''
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	907 indices = []
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	908 intersections = []
631 2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	909
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	910 for i in xrange(self.length()-1):
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	911 q1=self.__getitem__(i)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	912 q2=self.__getitem__(i+1)
771 bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	913 p = segmentLineIntersection(p1, p2, q1, q2)
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	914 if p is not None:
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	915 if q1.x != q2.x:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	916 ratio = (p.x-q1.x)/(q2.x-q1.x)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	917 elif q1.y != q2.y:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	918 ratio = (p.y-q1.y)/(q2.y-q1.y)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	919 else:
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	920 ratio = 0
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	921 indices.append(i+ratio)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	922 intersections.append(p)
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	923 return indices, intersections
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	924
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	925 def getTrajectoryInInterval(self, inter):
622 dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	926 'Returns all position between index inter.first and index.last (included)'
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	927 if inter.first >=0 and inter.last<= self.length():
622 dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	928 return Trajectory([self.positions[0][inter.first:inter.last+1],
dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	929 self.positions[1][inter.first:inter.last+1]])
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	930 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	931 return None
762 d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	932
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	933 def subSample(self, step):
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	934 'Returns the positions very step'
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	935 return Trajectory([self.positions[0][::step],
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	936 self.positions[1][::step]])
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	937
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	938 if shapelyAvailable:
781 7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	939 def getTrajectoryInPolygon(self, polygon, t2 = None):
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	940 '''Returns the trajectory built with the set of points inside the (shapely) polygon
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	941 The polygon could be a prepared polygon (faster) from prepared.prep
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	942
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	943 t2 is another trajectory (could be velocities)
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	944 which is filtered based on the first (self) 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	945 traj = Trajectory()
781 7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	946 inPolygon = []
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	947 for x, y in zip(self.positions[0], self.positions[1]):
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	948 inPolygon.append(polygon.contains(shapelyPoint(x, y)))
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	949 if inPolygon[-1]:
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	950 traj.addPositionXY(x, y)
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	951 traj2 = Trajectory()
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	952 if t2 is not None:
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	953 for inp, x, y in zip(inPolygon, t2.positions[0], t2.positions[1]):
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	954 if inp:
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	955 traj2.addPositionXY(x, y)
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	956 return traj, traj2
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	957
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	958 def proportionInPolygon(self, polygon, minProportion = 0.5):
781 7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	959 inPolygon = [polygon.contains(shapelyPoint(x, y)) for x, y in zip(self.positions[0], self.positions[1])]
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	960 lengthThreshold = float(self.length())*minProportion
781 7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	961 return sum(inPolygon) >= lengthThreshold
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	962 else:
781 7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	963 def getTrajectoryInPolygon(self, polygon, t2 = None):
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	964 '''Returns the trajectory built with the set of points inside the polygon
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	965 (array of Nx2 coordinates of the polygon vertices)'''
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	966 traj = Trajectory()
781 7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	967 inPolygon = []
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	968 for p in self:
781 7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	969 inPolygon.append(p.inPolygon(polygon))
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	970 if inPolygon[-1]:
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	971 traj.addPosition(p)
781 7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	972 traj2 = Trajectory()
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	973 if t2 is not None:
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	974 for inp, x, y in zip(inPolygon, t2.positions[0], t2.positions[1]):
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	975 if inp:
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	976 traj2.addPositionXY(p.x, p.y)
7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	977 return traj, traj2
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	978
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	979 def proportionInPolygon(self, polygon, minProportion = 0.5):
781 7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	980 inPolygon = [p.inPolygon(polygon) for p in self]
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	981 lengthThreshold = float(self.length())*minProportion
781 7c38250ddfc7 updated to deal with prepared polygons from shapely, and to extract the same positions from a second trajectory in a polygon (for velocities for example) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 780 diff changeset	982 return sum(inPolygon) >= lengthThreshold
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	983
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	984 @staticmethod
369 027e254f0b53 lcss subclass for indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 368 diff changeset	985 def lcss(t1, t2, lcss):
027e254f0b53 lcss subclass for indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 368 diff changeset	986 return lcss.compute(t1, t2)
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	987
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	988 class CurvilinearTrajectory(Trajectory):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	989 '''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	990 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	991 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	992
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	993 def __init__(self, S = None, Y = None, lanes = None):
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	994 if S is None or Y is 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	995 self.positions = [[],[]]
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	996 if S is not None and Y is not None and len(S) != len(Y):
543 cb213269d330 defensive code for curvilinear trajectory initialization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 542 diff changeset	997 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	998 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	999 self.positions = [S,Y]
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	1000 if lanes is 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	1001 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	1002 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	1003 self.lanes = lanes
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1004
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1005 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	1006 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	1007 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	1008 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	1009 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	1010 #elif isinstance( key, slice ):
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1011
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1012 def getSCoordinates(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1013 return self.getXCoordinates()
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1014
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1015 def getLanes(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1016 return self.lanes
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1017
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	1018 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	1019 self.addPositionXY(s,y)
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1020 self.lanes.append(lane)
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1021
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	1022 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	1023 '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	1024 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	1025
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1026 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	1027 self.setPositionXY(i, s, y)
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1028 if i < self.__len__():
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1029 self.lanes[i] = lane
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1030
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	1031 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	1032 diff = CurvilinearTrajectory()
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	1033 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	1034 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	1035 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	1036 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	1037 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	1038 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	1039 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	1040 return diff
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	1041
451 cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	1042 def getIntersections(self, S1, lane = None):
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	1043 '''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	1044 goes past the curvilinear coordinate S1
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	1045 (in provided lane if lane is not None)
771 bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	1046 Returns an empty list if there is no crossing'''
451 cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	1047 indices = []
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	1048 for i in xrange(self.length()-1):
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	1049 q1=self.__getitem__(i)
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	1050 q2=self.__getitem__(i+1)
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	1051 if q1[0] <= S1 < q2[0] and (lane is 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	1052 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	1053 return indices
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	1054
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1055 ##################
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1056 # Moving Objects
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1057 ##################
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1058
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	1059 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	1060 'car',
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1061 '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	1062 'motorcycle',
a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	1063 'bicycle',
185 c06379f25ab8 utilities for user types Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 184 diff changeset	1064 'bus',
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1065 'truck']
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1066
185 c06379f25ab8 utilities for user types Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 184 diff changeset	1067 userType2Num = utils.inverseEnumeration(userTypeNames)
c06379f25ab8 utilities for user types Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 184 diff changeset	1068
664 455f9b93819c added capability to set a videofilename to movingobject and interaction, renames interactiontype to collision in interaction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 663 diff changeset	1069 class MovingObject(STObject, VideoFilenameAddable):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	1070 '''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	1071 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	1072 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	1073 '''
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	1074
411 31604ef1cad4 added hog functions and the display of road user types if known Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 383 diff changeset	1075 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	1076 super(MovingObject, self).__init__(num, timeInterval)
16 9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	1077 self.positions = positions
345 fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1078 self.velocities = velocities
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	1079 self.geometry = geometry
54 c354d41ef7cd corrected code for usertype in movingobject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 49 diff changeset	1080 self.userType = userType
661 dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 660 diff changeset	1081 self.features = None
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	1082 # compute bounding polygon from trajectory
662 72174e66aba5 corrected bug that increased TTC by 1 frame and structure to store collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 661 diff changeset	1083
555 f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	1084 @staticmethod
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	1085 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	1086 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	1087 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	1088
622 dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	1089 @staticmethod
623 ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1090 def concatenate(obj1, obj2, num = None):
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1091 '''Concatenates two objects supposed to overlap temporally '''
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1092 commonTimeInterval = obj1.commonTimeInterval(obj2)
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1093 if commonTimeInterval.empty():
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1094 print('The two objects\' time intervals do not overlap: obj1 {} and obj2 {}'.format(obj1.getTimeInterval(), obj2.getTimeInterval()))
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1095 return None
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1096 else:
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	1097 if num is None:
623 ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1098 newNum = obj1.getNum()
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1099 else:
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1100 newNum = num
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1101 newTimeInterval = TimeInterval.union(obj1.getTimeInterval(), obj2.getTimeInterval())
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1102 # positions
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1103 positions = Trajectory()
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1104 for t in newTimeInterval:
663 65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1105 nTotal = 0.
623 ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1106 p = Point(0.,0.)
663 65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1107 for obj in [obj1, obj2]:
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1108 if obj.existsAtInstant(t):
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1109 if obj.hasFeatures():
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1110 n = len([f for f in obj.getFeatures() if f.existsAtInstant(t)])
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1111 else:
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1112 n = 1.
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1113 p += obj.getPositionAtInstant(t).multiply(n)
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1114 nTotal += n
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1115 assert nTotal>0, 'there should be at least one point for each instant'
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1116 positions.addPosition(p.divide(nTotal))
623 ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1117 # velocities: if any
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1118 if hasattr(obj1, 'velocities') and hasattr(obj2, 'velocities'):
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1119 velocities = Trajectory()
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1120 for t in newTimeInterval:
663 65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1121 nTotal = 0.
623 ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1122 p = Point(0.,0.)
663 65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1123 for obj in [obj1, obj2]:
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1124 if obj.existsAtInstant(t):
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1125 if obj.hasFeatures():
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1126 n = len([f for f in obj.getFeatures() if f.existsAtInstant(t)])
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1127 else:
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1128 n = 1.
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1129 p += obj.getVelocityAtInstant(t).multiply(n)
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1130 nTotal += n
623 ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1131 assert n>0, 'there should be at least one point for each instant'
663 65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1132 velocities.addPosition(p.divide(nTotal))
623 ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1133 else:
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1134 velocities = None
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1135 # TODO object envelop (polygon)
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1136 # user type
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1137 if obj1.getUserType() != obj2.getUserType():
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1138 print('The two moving objects have different user types: obj1 {} obj2 {}'.format(userTypeNames[obj1.getUserType()], userTypeNames[obj2.getUserType()]))
ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1139
663 65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1140 newObject = MovingObject(newNum, newTimeInterval, positions, velocities, userType = obj1.getUserType())
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1141 if obj1.hasFeatures() and obj2.hasFeatures():
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1142 newObject.features = obj1.getFeatures()+obj2.getFeatures()
65a867942eee modified concatenate to take into account features if available Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 662 diff changeset	1143 return newObject
622 dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	1144
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1145 def getObjectInTimeInterval(self, inter):
54 c354d41ef7cd corrected code for usertype in movingobject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 49 diff changeset	1146 '''Returns a new object extracted from self,
c354d41ef7cd corrected code for usertype in movingobject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 49 diff changeset	1147 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	1148 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	1149 if not intersection.empty():
b3a1c26e2f22 corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 96 diff changeset	1150 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	1151 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	1152 if self.velocities:
97 b3a1c26e2f22 corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 96 diff changeset	1153 obj.velocities = self.velocities.getTrajectoryInInterval(trajectoryInterval)
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1154 return obj
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1155 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1156 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	1157 return None
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1158
726 43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1159 def getObjectsInMask(self, mask, homography = None, minLength = 1):
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1160 '''Returns new objects made of the positions in the mask
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1161 mask is in the destination of the homography space'''
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1162 if homography is not None:
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1163 self.projectedPositions = self.positions.project(homography)
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1164 else:
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1165 self.projectedPositions = self.positions
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1166 def inMask(positions, i, mask):
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1167 p = positions[i]
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1168 return mask[p.y, p.x] != 0.
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1169
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1170 #subTimeIntervals self.getFirstInstant()+i
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1171 filteredIndices = [inMask(self.projectedPositions, i, mask) for i in range(int(self.length()))]
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1172 # 'connected components' in subTimeIntervals
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1173 l = 0
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1174 intervalLabels = []
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1175 prev = True
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1176 for i in filteredIndices:
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1177 if i:
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1178 if not prev: # new interval
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1179 l += 1
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1180 intervalLabels.append(l)
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1181 else:
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1182 intervalLabels.append(-1)
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1183 prev = i
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1184 intervalLabels = array(intervalLabels)
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1185 subObjects = []
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1186 for l in set(intervalLabels):
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1187 if l >= 0:
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1188 if sum(intervalLabels == l) >= minLength:
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1189 times = [self.getFirstInstant()+i for i in range(len(intervalLabels)) if intervalLabels[i] == l]
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1190 subTimeInterval = TimeInterval(min(times), max(times))
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1191 subObjects.append(self.getObjectInTimeInterval(subTimeInterval))
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1192
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1193 return subObjects
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1194
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1195 def getPositions(self):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1196 return self.positions
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1197
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1198 def getVelocities(self):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1199 return self.velocities
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1200
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	1201 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	1202 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	1203
335 3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1204 def getCurvilinearPositions(self):
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1205 if hasattr(self, 'curvilinearPositions'):
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1206 return self.curvilinearPositions
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1207 else:
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1208 return None
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1209
643 bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1210 def plotCurvilinearPositions(self, lane = None, options = '', withOrigin = False, **kwargs):
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1211 if hasattr(self, 'curvilinearPositions'):
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1212 if lane is None:
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1213 plot(list(self.getTimeInterval()), self.curvilinearPositions.positions[0], options, **kwargs)
660 994dd644f6ab corrected impact of warmUpLastInstant Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 659 diff changeset	1214 if withOrigin:
994dd644f6ab corrected impact of warmUpLastInstant Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 659 diff changeset	1215 plot([self.getFirstInstant()], [self.curvilinearPositions.positions[0][0]], 'ro', **kwargs)
643 bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1216 else:
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1217 instants = []
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1218 coords = []
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1219 for t, p in zip(self.getTimeInterval(), self.curvilinearPositions):
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1220 if p[2] == lane:
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1221 instants.append(t)
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1222 coords.append(p[0])
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1223 else:
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1224 instants.append(NaN)
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1225 coords.append(NaN)
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1226 plot(instants, coords, options, **kwargs)
660 994dd644f6ab corrected impact of warmUpLastInstant Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 659 diff changeset	1227 if withOrigin and len(instants)>0:
994dd644f6ab corrected impact of warmUpLastInstant Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 659 diff changeset	1228 plot([instants[0]], [coords[0]], 'ro', **kwargs)
643 bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1229 else:
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1230 print('Object {} has no curvilinear positions'.format(self.getNum()))
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1231
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	1232 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	1233 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	1234
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 259 diff changeset	1235 def setFeatures(self, features):
0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 259 diff changeset	1236 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	1237
661 dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 660 diff changeset	1238 def getFeatures(self):
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 660 diff changeset	1239 return self.features
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 660 diff changeset	1240
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 660 diff changeset	1241 def hasFeatures(self):
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 660 diff changeset	1242 return (self.features is not None)
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 660 diff changeset	1243
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 660 diff changeset	1244 def getFeature(self, i):
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 660 diff changeset	1245 if self.hasFeatures() and i<len(self.features):
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 660 diff changeset	1246 return self.features[i]
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 660 diff changeset	1247 else:
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 660 diff changeset	1248 return None
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 660 diff changeset	1249
666 93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1250 def getFeatureNumbers(self):
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1251 '''Returns the number of features at each instant
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1252 dict instant -> number of features'''
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1253 if self.hasFeatures():
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1254 featureNumbers = {}
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1255 for t in self.getTimeInterval():
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1256 n = 0
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1257 for f in self.getFeatures():
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1258 if f.existsAtInstant(t):
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1259 n += 1
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1260 featureNumbers[t]=n
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1261 return featureNumbers
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1262 else:
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1263 print('Object {} has no features loaded.'.format(self.getNum()))
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1264 return None
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1265
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1266 def getSpeeds(self, nInstantsIgnoredAtEnds = 0):
682 fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1267 speeds = self.getVelocities().norm()
fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1268 if nInstantsIgnoredAtEnds > 0:
fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1269 n = min(nInstantsIgnoredAtEnds, int(floor(self.length()/2.)))
fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1270 return speeds[n:-n]
fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1271 else:
fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1272 return speeds
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	1273
633 f410c8fb07b7 minor function to export speed as temporal indicator for display Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 631 diff changeset	1274 def getSpeedIndicator(self):
f410c8fb07b7 minor function to export speed as temporal indicator for display Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 631 diff changeset	1275 from indicators import SeverityIndicator
f410c8fb07b7 minor function to export speed as temporal indicator for display Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 631 diff changeset	1276 return SeverityIndicator('Speed', {t:self.getVelocityAtInstant(t).norm2() for t in self.getTimeInterval()})
f410c8fb07b7 minor function to export speed as temporal indicator for display Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 631 diff changeset	1277
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1278 def getPositionAt(self, i):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1279 return self.positions[i]
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1280
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1281 def getVelocityAt(self, i):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1282 return self.velocities[i]
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1283
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1284 def getPositionAtInstant(self, i):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1285 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	1286
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1287 def getVelocityAtInstant(self, i):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1288 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	1289
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1290 def getXCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1291 return self.positions.getXCoordinates()
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1292
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1293 def getYCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1294 return self.positions.getYCoordinates()
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1295
517 47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1296 def plot(self, options = '', withOrigin = False, timeStep = 1, withFeatures = False, **kwargs):
661 dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 660 diff changeset	1297 if withFeatures and self.hasFeatures():
dc70d9e711f5 some method name change and new methods for features in objects (MovingObject) and methods to access indicator values in interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 660 diff changeset	1298 for f in self.getFeatures():
517 47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1299 f.positions.plot('r', True, timeStep, **kwargs)
47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1300 self.positions.plot('bx-', True, timeStep, **kwargs)
47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1301 else:
47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1302 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	1303
621 582508610572 merge and updated consistently that imageheight does not seem necessary anymore for plotting on world image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 619 620 diff changeset	1304 def plotOnWorldImage(self, nPixelsPerUnitDistance, options = '', withOrigin = False, timeStep = 1, **kwargs):
582508610572 merge and updated consistently that imageheight does not seem necessary anymore for plotting on world image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 619 620 diff changeset	1305 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, **kwargs)
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	1306
626 35155ac2a294 corrected bugs, in particular to MovingObject.play Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 624 diff changeset	1307 def play(self, videoFilename, homography = None, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1.):
35155ac2a294 corrected bugs, in particular to MovingObject.play Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 624 diff changeset	1308 cvutils.displayTrajectories(videoFilename, [self], homography = homography, firstFrameNum = self.getFirstInstant(), lastFrameNumArg = self.getLastInstant(), undistort = undistort, intrinsicCameraMatrix = intrinsicCameraMatrix, distortionCoefficients = distortionCoefficients, undistortedImageMultiplication = undistortedImageMultiplication)
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	1309
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1310 def speedDiagnostics(self, framerate = 1., display = False):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1311 speeds = framerate*self.getSpeeds()
377 2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1312 coef = utils.linearRegression(range(len(speeds)), speeds)
379 f1a1923ddff9 minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 378 diff changeset	1313 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	1314 if display:
672 5473b7460375 moved and rationalized imports in modules Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 666 diff changeset	1315 from matplotlib.pyplot import figure, axis
377 2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1316 figure(1)
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	1317 self.plot()
379 f1a1923ddff9 minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 378 diff changeset	1318 axis('equal')
377 2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1319 figure(2)
2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1320 plot(list(self.getTimeInterval()), speeds)
2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1321
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1322 @staticmethod
653 107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1323 def minMaxDistance(obj1, obj2):
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1324 '''Computes the min max distance used for feature grouping'''
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1325 commonTimeInterval = obj1.commonTimeInterval(obj2)
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1326 if not commonTimeInterval.empty():
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1327 minDistance = (obj1.getPositionAtInstant(commonTimeInterval.first)-obj2.getPositionAtInstant(commonTimeInterval.first)).norm2()
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1328 maxDistance = minDistance
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1329 for t in list(commonTimeInterval)[1:]:
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1330 d = (obj1.getPositionAtInstant(t)-obj2.getPositionAtInstant(t)).norm2()
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1331 if d<minDistance:
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1332 minDistance = d
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1333 elif d>maxDistance:
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1334 maxDistance = d
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1335 return int(commonTimeInterval.length()), minDistance, maxDistance
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1336 else:
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1337 return int(commonTimeInterval.length()), None, None
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1338
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1339 @staticmethod
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1340 def distances(obj1, obj2, instant1, _instant2 = None):
653 107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1341 '''Returns the distances between all features of the 2 objects
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1342 at the same instant instant1
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1343 or at instant1 and instant2'''
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	1344 if _instant2 is None:
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1345 instant2 = instant1
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1346 else:
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1347 instant2 = _instant2
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1348 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	1349 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	1350 return cdist(positions1, positions2, metric = 'euclidean')
6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1351
6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1352 @staticmethod
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1353 def minDistance(obj1, obj2, instant1, instant2 = None):
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1354 return MovingObject.distances(obj1, obj2, instant1, instant2).min()
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1355
6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1356 @staticmethod
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1357 def maxDistance(obj1, obj2, instant, instant2 = None):
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1358 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	1359
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1360 def maxSize(self):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1361 '''Returns the max distance between features
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1362 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	1363 if hasattr(self, 'features'):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1364 nFeatures = -1
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1365 tMaxFeatures = 0
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1366 for t in self.getTimeInterval():
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1367 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	1368 if n > nFeatures:
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1369 nFeatures = n
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1370 tMaxFeatures = t
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1371 return MovingObject.maxDistance(self, self, tMaxFeatures)
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1372 else:
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1373 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	1374 return None
619 dc2d0a0d7fe1 merged code from Mohamed Gomaa Mohamed for the use of points of interests in mation pattern learning and motion prediction (TRB 2015) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 596 616 diff changeset	1375
550 5668af2ff515 minor naming Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 549 diff changeset	1376 def setRoutes(self, startRouteID, endRouteID):
5668af2ff515 minor naming Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 549 diff changeset	1377 self.startRouteID = startRouteID
5668af2ff515 minor naming Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 549 diff changeset	1378 self.endRouteID = endRouteID
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1379
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1380 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	1381 '''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	1382 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	1383 empty list if there is no crossing'''
631 2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1384 indices, intersections = self.positions.getIntersections(p1, p2)
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1385 return [t+self.getFirstInstant() for t in indices]
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	1386
631 2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1387 @staticmethod
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1388 def computePET(obj1, obj2, collisionDistanceThreshold):
635 6ae68383071e corrected issue with tests requiring shapely, adding a separate test file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 633 diff changeset	1389 '''Post-encroachment time based on distance threshold
6ae68383071e corrected issue with tests requiring shapely, adding a separate test file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 633 diff changeset	1390
6ae68383071e corrected issue with tests requiring shapely, adding a separate test file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 633 diff changeset	1391 Returns the smallest time difference when the object positions are within collisionDistanceThreshold'''
631 2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1392 #for i in xrange(int(obj1.length())-1):
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1393 # for j in xrange(int(obj2.length())-1):
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1394 # inter = segmentIntersection(obj1.getPositionAt(i), obj1.getPositionAt(i+1), obj2.getPositionAt(i), obj2.getPositionAt(i+1))
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1395 positions1 = [p.astuple() for p in obj1.getPositions()]
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1396 positions2 = [p.astuple() for p in obj2.getPositions()]
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1397 pets = zeros((int(obj1.length()), int(obj2.length())))
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1398 for i,t1 in enumerate(obj1.getTimeInterval()):
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1399 for j,t2 in enumerate(obj2.getTimeInterval()):
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1400 pets[i,j] = abs(t1-t2)
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1401 distances = cdist(positions1, positions2, metric = 'euclidean')
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1402 if distances.min() <= collisionDistanceThreshold:
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1403 return pets[distances <= collisionDistanceThreshold].min()
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1404 else:
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1405 return None
2d1d33ae1c69 major work on pPET and pet, issues remain for pPET computed with predicted trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 630 diff changeset	1406
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	1407 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	1408 '''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	1409 at constant speed'''
255 13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	1410 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	1411
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1412 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	1413 ''' 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	1414 (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	1415 curvilinear coordinates using alignment metadata.
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1416 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	1417 ======
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1418
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1419 Input:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1420 ======
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1421 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	1422 points (class Point).
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1423 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	1424 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	1425 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	1426
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1427 '''
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1428
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1429 self.curvilinearPositions = CurvilinearTrajectory()
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1430
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1431 #For each point
579 05c927c6d3cf curvilinear projection seems operational Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 578 diff changeset	1432 for i in xrange(int(self.length())):
05c927c6d3cf curvilinear projection seems operational Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 578 diff changeset	1433 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	1434
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1435 # Error handling
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	1436 if(result is None):
579 05c927c6d3cf curvilinear projection seems operational Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 578 diff changeset	1437 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	1438 else:
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1439 [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	1440 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	1441
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1442 ## 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	1443 #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	1444 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	1445 ## Recalculate projected point to new lane
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1446 lanes = self.curvilinearPositions.getLanes()
fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1447 if(lanes != smoothed_lanes):
579 05c927c6d3cf curvilinear projection seems operational Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 578 diff changeset	1448 for i in xrange(len(lanes)):
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1449 if(lanes[i] != smoothed_lanes[i]):
fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1450 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	1451
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1452 # Error handling
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	1453 if(result is None):
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	1454 ## 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	1455 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	1456 else:
579 05c927c6d3cf curvilinear projection seems operational Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 578 diff changeset	1457 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	1458 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	1459
562 259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1460 def computeSmoothTrajectory(self, minCommonIntervalLength):
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1461 '''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	1462 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	1463
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1464 Warning work in progress
562 259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1465 TODO? not use the first/last 1-.. positions'''
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1466 nFeatures = len(self.features)
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1467 if nFeatures == 0:
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1468 print('Empty object features\nCannot compute smooth trajectory')
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1469 else:
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1470 # compute the relative position vectors
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1471 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	1472 for i in xrange(nFeatures):
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1473 for j in xrange(i):
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1474 fi = self.features[i]
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1475 fj = self.features[j]
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1476 inter = fi.commonTimeInterval(fj)
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1477 if inter.length() >= minCommonIntervalLength:
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1478 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	1479 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	1480 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	1481 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	1482 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	1483 relativePositions[(j,i)] = -relativePositions[(i,j)]
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1484
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	1485 ###
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	1486 # 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	1487 ###
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1488 def classifyUserTypeSpeedMotorized(self, threshold, aggregationFunc = median, nInstantsIgnoredAtEnds = 0):
345 fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1489 '''Classifies slow and fast road users
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1490 slow: non-motorized -> pedestrians
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 583 diff changeset	1491 fast: motorized -> cars
608 078adacd72a4 moving.py integrating Mohamed's comments refactored Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 607 diff changeset	1492
078adacd72a4 moving.py integrating Mohamed's comments refactored Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 607 diff changeset	1493 aggregationFunc can be any function that can be applied to a vector of speeds, including percentile:
078adacd72a4 moving.py integrating Mohamed's comments refactored Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 607 diff changeset	1494 aggregationFunc = lambda x: percentile(x, percentileFactor) # where percentileFactor is 85 for 85th percentile'''
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1495 speeds = self.getSpeeds(nInstantsIgnoredAtEnds)
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	1496 if aggregationFunc(speeds) >= threshold:
345 fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1497 self.setUserType(userType2Num['car'])
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1498 else:
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1499 self.setUserType(userType2Num['pedestrian'])
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1500
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1501 def classifyUserTypeSpeed(self, speedProbabilities, aggregationFunc = median, nInstantsIgnoredAtEnds = 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	1502 '''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	1503 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	1504 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	1505 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	1506
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	1507 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	1508 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	1509 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	1510 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	1511 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	1512 return x'''
626 35155ac2a294 corrected bugs, in particular to MovingObject.play Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 624 diff changeset	1513 if not hasattr(self, 'aggregatedSpeed'):
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1514 self.aggregatedSpeed = aggregationFunc(self.getSpeeds(nInstantsIgnoredAtEnds))
581 10e8a9f2bd9f change for python 2.6 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 579 diff changeset	1515 userTypeProbabilities = {}
10e8a9f2bd9f change for python 2.6 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 579 diff changeset	1516 for userTypename in speedProbabilities:
10e8a9f2bd9f change for python 2.6 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 579 diff changeset	1517 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	1518 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	1519 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	1520
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1521 def initClassifyUserTypeHoGSVM(self, aggregationFunc, pedBikeCarSVM, bikeCarSVM = None, pedBikeSpeedTreshold = float('Inf'), bikeCarSpeedThreshold = float('Inf'), nInstantsIgnoredAtEnds = 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	1522 '''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	1523
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1524 TODO? compute speed for longest feature?'''
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1525 self.aggregatedSpeed = aggregationFunc(self.getSpeeds(nInstantsIgnoredAtEnds))
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1526 if self.aggregatedSpeed < pedBikeSpeedTreshold or bikeCarSVM is None:
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1527 self.appearanceClassifier = pedBikeCarSVM
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1528 elif self.aggregatedSpeed < bikeCarSpeedThreshold:
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1529 self.appearanceClassifier = bikeCarSVM
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1530 else:
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1531 class CarClassifier:
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1532 def predict(self, hog):
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1533 return userType2Num['car']
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1534 self.appearanceClassifier = CarClassifier()
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1535
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	1536 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	1537
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1538 def classifyUserTypeHoGSVMAtInstant(self, img, instant, homography, width, height, px = 0.2, py = 0.2, minNPixels = 800):
520 fd9641cbd24b added function to classify object at instant from SVM Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 517 diff changeset	1539 '''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	1540 applies the SVM model on it'''
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1541 croppedImg, yCropMin, yCropMax, xCropMin, xCropMax = cvutils.imageBox(img, self, instant, homography, width, height, px, py, minNPixels)
685 94b291a5f933 several updates for display Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 682 diff changeset	1542 if croppedImg is not None and len(croppedImg) > 0:
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1543 hog = cvutils.HOG(croppedImg)#HOG(image, rescaleSize = (64, 64), orientations=9, pixelsPerCell=(8, 8), cellsPerBlock=(2, 2), visualize=False, normalize=False)
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1544 self.userTypes[instant] = int(self.appearanceClassifier.predict(hog))
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	1545 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	1546 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	1547
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1548 def classifyUserTypeHoGSVM(self, pedBikeCarSVM = None, width = 0, height = 0, homography = None, images = None, bikeCarSVM = None, pedBikeSpeedTreshold = float('Inf'), bikeCarSpeedThreshold = float('Inf'), minSpeedEquiprobable = -1, speedProbabilities = None, aggregationFunc = median, nInstantsIgnoredAtEnds = 0, px = 0.2, py = 0.2, minNPixels = 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	1549 '''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	1550 (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	1551
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1552 images is a dictionary of images indexed by instant
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	1553 With default parameters, the general (ped-bike-car) classifier will be used
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1554
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1555 Considered categories are the keys of speedProbabilities'''
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1556 if not hasattr(self, 'aggregatedSpeed') or not hasattr(self, 'userTypes'):
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	1557 print('Initilize the data structures for classification by HoG-SVM')
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1558 self.initClassifyUserTypeHoGSVM(aggregationFunc, pedBikeCarSVM, bikeCarSVM, pedBikeSpeedTreshold, bikeCarSpeedThreshold, nInstantsIgnoredAtEnds)
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	1559
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1560 if len(self.userTypes) != self.length() and images is not None: # if classification has not been done previously
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	1561 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	1562 if t not in self.userTypes:
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1563 self.classifyUserTypeHoGSVMAtInstant(images[t], t, homography, width, height, px, py, minNPixels)
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	1564 # compute P(Speed\|Class)
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1565 if speedProbabilities is None or self.aggregatedSpeed < minSpeedEquiprobable: # 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	1566 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	1567 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	1568 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	1569 # result is P(Class\|Appearance) x P(Speed\|Class)
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1570 nInstantsUserType = {userTypeNum: 0 for userTypeNum in userTypeProbabilities}# number of instants the object is classified as userTypename
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	1571 for t in self.userTypes:
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1572 nInstantsUserType[self.userTypes[t]] = nInstantsUserType.get(self.userTypes[t], 0) + 1
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1573 for userTypeNum in userTypeProbabilities:
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1574 userTypeProbabilities[userTypeNum] *= nInstantsUserType[userTypeNum]
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	1575 # 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	1576 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	1577
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	1578 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	1579 '''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	1580 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	1581 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	1582
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	1583 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	1584 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	1585 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	1586 if not hasattr(self, projectedPositions):
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	1587 if homography is not None:
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	1588 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	1589 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	1590 self.projectedPositions = obj.positions
523 ce4eaabacc26 modified internal implementation of user type for classifyUserTypeArea Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 522 diff changeset	1591 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	1592 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	1593 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	1594 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	1595 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	1596 # 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	1597 return possibleUserTypes
520 fd9641cbd24b added function to classify object at instant from SVM Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 517 diff changeset	1598
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1599 @staticmethod
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1600 def collisionCourseDotProduct(movingObject1, movingObject2, instant):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1601 '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	1602 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	1603 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	1604 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	1605
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1606 @staticmethod
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1607 def collisionCourseCosine(movingObject1, movingObject2, instant):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1608 '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	1609 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	1610 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	1611
587 cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	1612
cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	1613 ##################
588 c5406edbcf12 added loading ground truth annotations (ground truth) from polytrack format Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 587 diff changeset	1614 # Annotations
587 cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	1615 ##################
cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	1616
768 f8e0a8ea8402 updated the bounding box code (the name so that it is general for ground truth and UT) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 762 diff changeset	1617 class BBMovingObject(MovingObject):
f8e0a8ea8402 updated the bounding box code (the name so that it is general for ground truth and UT) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 762 diff changeset	1618 '''Class for a moving object represented as a bounding box
f8e0a8ea8402 updated the bounding box code (the name so that it is general for ground truth and UT) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 762 diff changeset	1619 used for series of ground truth annotations using bounding boxes
f8e0a8ea8402 updated the bounding box code (the name so that it is general for ground truth and UT) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 762 diff changeset	1620 and for the output of Urban Tracker http://www.jpjodoin.com/urbantracker/
590 0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1621
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1622 By default in image space
768 f8e0a8ea8402 updated the bounding box code (the name so that it is general for ground truth and UT) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 762 diff changeset	1623
f8e0a8ea8402 updated the bounding box code (the name so that it is general for ground truth and UT) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 762 diff changeset	1624 Its center is the center of the box (generalize to other shapes?)
f8e0a8ea8402 updated the bounding box code (the name so that it is general for ground truth and UT) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 762 diff changeset	1625 (computed after projecting if homography available)
588 c5406edbcf12 added loading ground truth annotations (ground truth) from polytrack format Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 587 diff changeset	1626 '''
587 cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	1627
589 5800a87f11ae corrected one bug and changed attribute names Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 588 diff changeset	1628 def __init__(self, num = None, timeInterval = None, topLeftPositions = None, bottomRightPositions = None, userType = userType2Num['unknown']):
768 f8e0a8ea8402 updated the bounding box code (the name so that it is general for ground truth and UT) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 762 diff changeset	1629 super(BBMovingObject, 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	1630 self.topLeftPositions = topLeftPositions.getPositions()
5800a87f11ae corrected one bug and changed attribute names Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 588 diff changeset	1631 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	1632
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1633 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	1634 self.positions = self.topLeftPositions.add(self.bottomRightPositions).multiply(0.5)
636 3058e00887bc removed all issues because of tests with None, using is instead of == or != Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 635 diff changeset	1635 if homography is not None:
590 0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1636 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	1637
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1638 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	1639 '''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	1640 with threshold matchingDistance
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1641 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	1642 (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	1643 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	1644 if d < matchingDistance:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1645 return d
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1646 else:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1647 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	1648
723 e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1649 def computeClearMOT(annotations, objects, matchingDistance, firstInstant, lastInstant, returnMatches = False, debug = False):
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1650 '''Computes the CLEAR MOT metrics
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1651
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1652 Reference:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1653 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	1654
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	1655 objects and annotations are supposed to in the same space
768 f8e0a8ea8402 updated the bounding box code (the name so that it is general for ground truth and UT) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 762 diff changeset	1656 current implementation is BBMovingObject (bounding boxes)
592 985a3021cff2 first match table implementation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 590 diff changeset	1657 mathingDistance is threshold on matching between annotation and object
985a3021cff2 first match table implementation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 590 diff changeset	1658
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1659 TO: tracker output (objects)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1660 GT: ground truth (annotations)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1661
624 bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	1662 Output: returns motp, mota, mt, mme, fpt, gt
bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	1663 mt number of missed GT.frames (sum of the number of GT not detected in each frame)
bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	1664 mme number of mismatches
bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	1665 fpt number of false alarm.frames (tracker objects without match in each frame)
bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	1666 gt number of GT.frames
bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	1667
723 e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1668 if returnMatches is True, return as 2 new arguments the GT and TO matches
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1669 matches is a dict
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1670 matches[i] is the list of matches for GT/TO i
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1671 the list of matches is a dict, indexed by time, for the TO/GT id matched at time t
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1672 (an instant t not present in matches[i] at which GT/TO exists means a missed detection or false alarm)
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1673
624 bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	1674 TODO: Should we use the distance as weights or just 1/0 if distance below matchingDistance?
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1675 (add argument useDistanceForWeights = False)'''
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1676 from munkres import Munkres
592 985a3021cff2 first match table implementation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 590 diff changeset	1677
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1678 munk = Munkres()
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1679 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	1680 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	1681 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	1682 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	1683 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	1684 mme = 0 # number of mismatches
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1685 matches = {} # match[i] is the tracker track associated with GT i (using object references)
723 e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1686 if returnMatches:
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1687 gtMatches = {a.getNum():{} for a in annotations}
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1688 toMatches = {o.getNum():{} for o in objects}
592 985a3021cff2 first match table implementation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 590 diff changeset	1689 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	1690 previousMatches = matches.copy()
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1691 # 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	1692 toDelete = []
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1693 for a in matches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1694 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	1695 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	1696 if d < matchingDistance:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1697 dist += d
592 985a3021cff2 first match table implementation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 590 diff changeset	1698 else:
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1699 toDelete.append(a)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1700 else:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1701 toDelete.append(a)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1702 for a in toDelete:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1703 del matches[a]
593 e2a873e08568 non-working clear mot metrics Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 592 diff changeset	1704
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1705 # match all unmatched GT-TO
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1706 matchedGTs = matches.keys()
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1707 matchedTOs = matches.values()
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1708 costs = []
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1709 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	1710 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	1711 nGTs = len(matchedGTs)+len(unmatchedGTs)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1712 nTOs = len(matchedTOs)+len(unmatchedTOs)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1713 if len(unmatchedTOs) > 0:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1714 for a in unmatchedGTs:
595 17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	1715 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	1716 if len(costs) > 0:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1717 newMatches = munk.compute(costs)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1718 for k,v in newMatches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1719 if costs[k][v] < matchingDistance:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1720 matches[unmatchedGTs[k]]=unmatchedTOs[v]
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1721 dist += costs[k][v]
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1722 if debug:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1723 print('{} '.format(t)+', '.join(['{} {}'.format(k.getNum(), v.getNum()) for k,v in matches.iteritems()]))
723 e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1724 if returnMatches:
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1725 for a,o in matches.iteritems():
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1726 gtMatches[a.getNum()][t] = o.getNum()
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1727 toMatches[o.getNum()][t] = a.getNum()
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1728
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1729 # compute metrics elements
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1730 ct += len(matches)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1731 mt += nGTs-len(matches)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1732 fpt += nTOs-len(matches)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1733 gt += nGTs
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1734 # compute mismatches
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1735 # 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	1736 mismatches = []
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1737 for a in matches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1738 if a in previousMatches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1739 if matches[a] != previousMatches[a]:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1740 mismatches.append(a)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1741 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	1742 mismatches.append(matches[a])
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1743 for a in previousMatches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1744 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	1745 mismatches.append(previousMatches[a])
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1746 if debug:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1747 for mm in set(mismatches):
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1748 print type(mm), mm.getNum()
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1749 # 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	1750 mme += len(set(mismatches))
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1751
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1752 if ct > 0:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1753 motp = dist/ct
593 e2a873e08568 non-working clear mot metrics Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 592 diff changeset	1754 else:
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	1755 motp = None
595 17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	1756 if gt > 0:
17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	1757 mota = 1.-float(mt+fpt+mme)/gt
17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	1758 else:
17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	1759 mota = None
723 e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1760 if returnMatches:
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1761 return motp, mota, mt, mme, fpt, gt, gtMatches, toMatches
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1762 else:
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	1763 return motp, mota, mt, mme, fpt, gt
593 e2a873e08568 non-working clear mot metrics Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 592 diff changeset	1764
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1765 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	1766 '''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	1767 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	1768 figure()
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1769 for obj in objects:
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	1770 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	1771 axis('equal')
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1772
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1773
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1774 if __name__ == "__main__":
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1775 import doctest
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1776 import unittest
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1777 suite = doctest.DocFileSuite('tests/moving.txt')
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1778 #suite = doctest.DocTestSuite()
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1779 unittest.TextTestRunner().run(suite)
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1780 #doctest.testmod()
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	1781 #doctest.testfile("example.txt")
635 6ae68383071e corrected issue with tests requiring shapely, adding a separate test file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 633 diff changeset	1782 if shapelyAvailable:
6ae68383071e corrected issue with tests requiring shapely, adding a separate test file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 633 diff changeset	1783 suite = doctest.DocFileSuite('tests/moving_shapely.txt')
6ae68383071e corrected issue with tests requiring shapely, adding a separate test file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 633 diff changeset	1784 unittest.TextTestRunner().run(suite)

Mercurial Hosting > traffic-intelligence

annotate python/moving.py @ 781:7c38250ddfc7 dev