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

annotate trafficintelligence/moving.py @ 1213:3f2214125164

work in progress

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Wed, 03 May 2023 14:58:26 -0400
parents	af329f3330ba
children	01c24c1cdb70

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
1029 c6cf75a2ed08 reorganization of imports Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1028 diff changeset	4 import copy
c6cf75a2ed08 reorganization of imports Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1028 diff changeset	5 from math import sqrt, atan2, cos, sin
0 aed8eb63cdde initial commit with non-functional python code for NGSIM Nicolas Saunier <nico@confins.net> parents: diff changeset	6
1207 36f0d18e1fad work in progress on loading kitti (issue with int) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1205 diff changeset	7 from numpy import median, mean, array, arange, zeros, ones, hypot, NaN, std, floor, ceil, float32, argwhere, minimum, issubdtype, integer as npinteger
1210 1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	8 from matplotlib.pyplot import plot, text, arrow
672 5473b7460375 moved and rationalized imports in modules Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 666 diff changeset	9 from scipy.stats import scoreatpercentile
5473b7460375 moved and rationalized imports in modules Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 666 diff changeset	10 from scipy.spatial.distance import cdist
877 d1ff6917d082 added savitzky golay filter for accelerations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 869 diff changeset	11 from scipy.signal import savgol_filter
1029 c6cf75a2ed08 reorganization of imports Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1028 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
1030 aafbc0bab925 moved method around to avoid cross-dependencies Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	21 from trafficintelligence import utils, cvutils
aafbc0bab925 moved method around to avoid cross-dependencies Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	22 from trafficintelligence.base import VideoFilenameAddable
aafbc0bab925 moved method around to avoid cross-dependencies Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1029 diff changeset	23
0 aed8eb63cdde initial commit with non-functional python code for NGSIM Nicolas Saunier <nico@confins.net> parents: diff changeset	24
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	25 class Interval(object):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	26 '''Generic interval: a subset of real numbers (not iterable)'''
26 54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	27 def __init__(self, first=0, last=-1, revert = False):
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	28 if revert and last<first:
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	29 self.first=last
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	30 self.last=first
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	31 else:
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	32 self.first=first
54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	33 self.last=last
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	34
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	35 def __str__(self):
1064 cbc026dacf0b changed interval string representation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1044 diff changeset	36 return '{0}-{1}'.format(self.first, self.last)
104 13187af8622d finally added the representation of intervals Nicolas Saunier <nico@confins.net> parents: 98 diff changeset	37
13187af8622d finally added the representation of intervals Nicolas Saunier <nico@confins.net> parents: 98 diff changeset	38 def __repr__(self):
13187af8622d finally added the representation of intervals Nicolas Saunier <nico@confins.net> parents: 98 diff changeset	39 return self.__str__()
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	40
776 84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	41 def __eq__(self, other):
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	42 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	43
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	44 def empty(self):
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	45 return self.first > self.last
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	46
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	47 def center(self):
619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	48 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	49
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	50 def length(self):
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	51 '''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	52 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	53
96 9928c2fa72cc added equal method to intervals Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 93 diff changeset	54 def equal(self, i2):
9928c2fa72cc added equal method to intervals Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 93 diff changeset	55 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	56
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	57 def getList(self):
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	58 return [self.first, self.last]
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 contains(self, instant):
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	61 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	62
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	63 def inside(self, interval2):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	64 '''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	65 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	66
785 3aa6102ccc12 addressed issues with ground truth annotations shifted in time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 781 diff changeset	67 def shift(self, offset):
3aa6102ccc12 addressed issues with ground truth annotations shifted in time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 781 diff changeset	68 self.first += offset
3aa6102ccc12 addressed issues with ground truth annotations shifted in time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 781 diff changeset	69 self.last += offset
3aa6102ccc12 addressed issues with ground truth annotations shifted in time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 781 diff changeset	70
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	71 @classmethod
1064 cbc026dacf0b changed interval string representation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1044 diff changeset	72 def parse(cls, s):
cbc026dacf0b changed interval string representation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1044 diff changeset	73 if '-' in s:
cbc026dacf0b changed interval string representation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1044 diff changeset	74 tmp = s.split('-')
cbc026dacf0b changed interval string representation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1044 diff changeset	75 if len(tmp) == 2:
cbc026dacf0b changed interval string representation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1044 diff changeset	76 return cls(int(tmp[0]), int(tmp[1])) # TODO with floats?
cbc026dacf0b changed interval string representation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1044 diff changeset	77 print(s+' is not a valid representation of an interval')
cbc026dacf0b changed interval string representation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1044 diff changeset	78 return None
cbc026dacf0b changed interval string representation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1044 diff changeset	79
cbc026dacf0b changed interval string representation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1044 diff changeset	80 @classmethod
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	81 def union(cls, interval1, interval2):
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	82 '''Smallest interval comprising self and interval2'''
1076 108c5dc4e34a Correcting loaders Wendlasida parents: 1075 diff changeset	83 return cls(min(interval1.first, interval2.first), max(interval1.last, interval2.last))
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	84
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	85 @classmethod
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	86 def intersection(cls, interval1, interval2):
104 13187af8622d finally added the representation of intervals Nicolas Saunier <nico@confins.net> parents: 98 diff changeset	87 '''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	88 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	89
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	90 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	91 if not Interval.intersection(self, interval2).empty():
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	92 return 0
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	93 elif self.first > interval2.last:
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	94 return self.first - interval2.last
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	95 elif self.last < interval2.first:
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	96 return interval2.first - self.last
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	97 else:
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	98 return None
571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	99
727 c6d4ea05a2d0 adding ability to deal with multivariate indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 726 diff changeset	100 @classmethod
c6d4ea05a2d0 adding ability to deal with multivariate indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 726 diff changeset	101 def unionIntervals(cls, intervals):
c6d4ea05a2d0 adding ability to deal with multivariate indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 726 diff changeset	102 '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	103 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	104 for i in intervals[1:]:
c6d4ea05a2d0 adding ability to deal with multivariate indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 726 diff changeset	105 inter = cls.union(inter, i)
c6d4ea05a2d0 adding ability to deal with multivariate indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 726 diff changeset	106 return inter
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	107
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	108
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	109 class TimeInterval(Interval):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	110 '''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	111
e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	112 For example: based on frame numbers (hence the modified length method)
828 14e4ad7c7420 work on merging data for synchronized views Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 812 diff changeset	113 It may be modified directly by setting first and last
14e4ad7c7420 work on merging data for synchronized views Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 812 diff changeset	114 It also (mostly) works with datetime.datetime'''
26 54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	115
828 14e4ad7c7420 work on merging data for synchronized views Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 812 diff changeset	116 def __init__(self, first=0, last=-1, revert = False):
14e4ad7c7420 work on merging data for synchronized views Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 812 diff changeset	117 super(TimeInterval, self).__init__(first, last, revert)
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	118
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	119 @staticmethod
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	120 def fromInterval(inter):
df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	121 return TimeInterval(inter.first, inter.last)
26 54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	122
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	123 def __getitem__(self, i):
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	124 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	125 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	126 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	127 else:
978 184f1dd307f9 corrected print and exception statements for Python 3 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 959 diff changeset	128 raise TypeError("Invalid argument type.")
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	129 #elif isinstance( key, slice ):
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	130
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	131 def __iter__(self):
107 916678481896 corrected bug for TimeInterval interation and added corresponding test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 105 diff changeset	132 self.iterInstantNum = -1
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	133 return self
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	134
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	135 def __next__(self):
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	136 if self.iterInstantNum >= self.length()-1:
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	137 raise StopIteration
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	138 else:
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	139 self.iterInstantNum += 1
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	140 return self[self.iterInstantNum]
44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	141
26 54d9cb0c902b generalized intervals Nicolas Saunier <nico@confins.net> parents: 25 diff changeset	142 def length(self):
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	143 '''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	144 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	145
688 f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	146 def __len__(self):
f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	147 return self.length()
f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	148
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	149 # class BoundingPolygon:
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	150 # '''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	151 # with methods to create intersection, unions...
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	152 # '''
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	153 # We will use the polygon class of Shapely
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	154
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	155 class STObject(object):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	156 '''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	157 (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	158
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	159 It may not mean that the object is defined
6 597d61c1eebe minor doc update Nicolas Saunier <nico@confins.net> parents: 2 diff changeset	160 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	161
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	162 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	163 self.num = num
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	164 self.timeInterval = timeInterval
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	165 self.boundingPolygon = boundingPolygon
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	166
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	167 def empty(self):
688 f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	168 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	169
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	170 def getNum(self):
211 ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	171 return self.num
ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	172
688 f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	173 def __len__(self):
f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	174 return self.timeInterval.length()
f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	175
f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	176 def length(self):
f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	177 return self.timeInterval.length()
f2b52355a286 made length a STObject method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 685 diff changeset	178
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	179 def getFirstInstant(self):
40 9f16aee24b7e corrected bug for first and last of TimeInterval Nicolas Saunier <nico@confins.net> parents: 39 diff changeset	180 return self.timeInterval.first
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	181
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	182 def getLastInstant(self):
40 9f16aee24b7e corrected bug for first and last of TimeInterval Nicolas Saunier <nico@confins.net> parents: 39 diff changeset	183 return self.timeInterval.last
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	184
1001 cc7c6b821ae6 added methods for setting instants Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 998 diff changeset	185 def setFirstInstant(self, t):
cc7c6b821ae6 added methods for setting instants Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 998 diff changeset	186 if t <= self.timeInterval.last:
cc7c6b821ae6 added methods for setting instants Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 998 diff changeset	187 self.timeInterval.first = t
cc7c6b821ae6 added methods for setting instants Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 998 diff changeset	188 else:
cc7c6b821ae6 added methods for setting instants Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 998 diff changeset	189 print('new first instant is after last, not changing')
cc7c6b821ae6 added methods for setting instants Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 998 diff changeset	190
cc7c6b821ae6 added methods for setting instants Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 998 diff changeset	191 def setLastInstant(self, t):
cc7c6b821ae6 added methods for setting instants Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 998 diff changeset	192 if t >= self.timeInterval.first:
cc7c6b821ae6 added methods for setting instants Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 998 diff changeset	193 self.timeInterval.last = t
cc7c6b821ae6 added methods for setting instants Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 998 diff changeset	194 else:
cc7c6b821ae6 added methods for setting instants Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 998 diff changeset	195 print('new last instant is before first, not changing')
cc7c6b821ae6 added methods for setting instants Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 998 diff changeset	196
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	197 def getTimeInterval(self):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	198 return self.timeInterval
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	199
108 6efe470ea5e5 added test existsAtInstant to STObject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 107 diff changeset	200 def existsAtInstant(self, t):
6efe470ea5e5 added test existsAtInstant to STObject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 107 diff changeset	201 return self.timeInterval.contains(t)
6efe470ea5e5 added test existsAtInstant to STObject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 107 diff changeset	202
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	203 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	204 return TimeInterval.intersection(self.getTimeInterval(), obj2.getTimeInterval())
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	205
785 3aa6102ccc12 addressed issues with ground truth annotations shifted in time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 781 diff changeset	206 def shiftTimeInterval(self, offset):
3aa6102ccc12 addressed issues with ground truth annotations shifted in time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 781 diff changeset	207 self.timeInterval.shift(offset)
3aa6102ccc12 addressed issues with ground truth annotations shifted in time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 781 diff changeset	208
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	209 class Point(object):
25 28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	210 def __init__(self, x, y):
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	211 self.x = x
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	212 self.y = y
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	213
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	214 def __str__(self):
675 ab3fdff42624 corrected old format for Point.__str__ Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 674 diff changeset	215 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	216
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	217 def __repr__(self):
98 b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	218 return self.__str__()
25 28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	219
776 84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	220 def __eq__(self, other):
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	221 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	222
184 d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	223 def __add__(self, other):
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	224 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	225
25 28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	226 def __sub__(self, other):
28e546861263 added Point class and modified trajectory accordingly Nicolas Saunier <nico@confins.net> parents: 23 diff changeset	227 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	228
451 cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	229 def __neg__(self):
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	230 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	231
1106 799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	232 def __mul__(self, alpha):
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	233 'Warning, returns a new Point'
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	234 return Point(self.xalpha, self.yalpha)
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	235
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	236 def divide(self, alpha):
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	237 'Warning, returns a new Point'
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	238 return Point(self.x/alpha, self.y/alpha)
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	239
575 13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	240 def __getitem__(self, i):
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	241 if i == 0:
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	242 return self.x
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	243 elif i == 1:
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	244 return self.y
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	245 else:
13df64a9ff9d added function to access point class as 2D list Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 574 diff changeset	246 raise IndexError()
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	247
690 463150a8e129 minor updates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 688 diff changeset	248 def orthogonal(self, clockwise = True):
463150a8e129 minor updates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 688 diff changeset	249 'Returns the orthogonal vector'
463150a8e129 minor updates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 688 diff changeset	250 if clockwise:
463150a8e129 minor updates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 688 diff changeset	251 return Point(self.y, -self.x)
463150a8e129 minor updates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 688 diff changeset	252 else:
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	253 return Point(-self.y, self.x)
451 cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	254
1106 799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	255 def normalize(self):
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	256 return self.divide(self.norm2())
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	257
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	258 def projectLocal(self, v, clockwise = True):
05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	259 'Projects point projected on v, v.orthogonal()'
1106 799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	260 e1 = v.normalize()
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	261 e2 = e1.orthogonal(clockwise)
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	262 return Point(Point.dot(self, e1), Point.dot(self, e2))
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	263
05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	264 def rotate(self, theta):
05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	265 return Point(self.xcos(theta)-self.ysin(theta), self.xsin(theta)+self.ycos(theta))
1109 6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	266
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	267 def plot(self, options = 'o', **kwargs):
211 ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	268 plot([self.x], [self.y], options, **kwargs)
41 eb78c6edc0c8 added drawing for Point Nicolas Saunier <nico@confins.net> parents: 40 diff changeset	269
771 bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	270 @staticmethod
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	271 def plotSegment(p1, p2, options = 'o', withOrigin = True, **kwargs):
771 bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	272 plot([p1.x, p2.x], [p1.y, p2.y], options, **kwargs)
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	273 p1.plot('or', **kwargs)
771 bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	274
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	275 def angle(self):
05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	276 return atan2(self.y, self.x)
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	277
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	278 def norm2Squared(self):
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	279 '''2-norm distance (Euclidean distance)'''
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	280 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	281
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	282 def norm2(self):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	283 '''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	284 return sqrt(self.norm2Squared())
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	285
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	286 def norm1(self):
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	287 return abs(self.x)+abs(self.y)
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	288
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	289 def normMax(self):
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	290 return max(abs(self.x),abs(self.y))
f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	291
64 c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	292 def aslist(self):
c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	293 return [self.x, self.y]
c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	294
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	295 def astuple(self):
e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	296 return (self.x, self.y)
e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	297
223 c31722fcc9de minor modifications for integer drawing in OpenCV Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 214 diff changeset	298 def asint(self):
c31722fcc9de minor modifications for integer drawing in OpenCV Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 214 diff changeset	299 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	300
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	301 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	302 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	303 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	304
936 56cc8a1f7082 removed all old versions of projection methods Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 935 diff changeset	305 def homographyProject(self, homography):
56cc8a1f7082 removed all old versions of projection methods Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 935 diff changeset	306 projected = cvutils.homographyProject(array([[self.x], [self.y]]), homography)
98 b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	307 return Point(projected[0], projected[1])
b85912ab4064 refactored projection functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 97 diff changeset	308
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	309 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	310 '''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	311 (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	312
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	313 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	314
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	315 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	316
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	317 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	318 counter = 0;
5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	319
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	320 p1 = polygon[0,:];
79 5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	321 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	322 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	323 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	324 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	325 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	326 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	327 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	328 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	329 counter+=1;
5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	330 p1=p2
5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	331 return (counter%2 == 1);
5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	332
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	333 @staticmethod
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	334 def fromList(p):
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	335 return Point(p[0], p[1])
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	336
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	337 @staticmethod
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	338 def dot(p1, p2):
89 f88a19695bba added inner product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 83 diff changeset	339 'Scalar product'
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	340 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	341
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	342 @staticmethod
90 f84293ad4611 renamed inner to cross product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 89 diff changeset	343 def cross(p1, p2):
f84293ad4611 renamed inner to cross product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 89 diff changeset	344 'Cross product'
89 f88a19695bba added inner product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 83 diff changeset	345 return p1.xp2.y-p1.yp2.x
f88a19695bba added inner product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 83 diff changeset	346
f88a19695bba added inner product Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 83 diff changeset	347 @staticmethod
987 f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 982 diff changeset	348 def parallel(p1, p2):
f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 982 diff changeset	349 return Point.cross(p1, p2) == 0.
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	350
987 f026ce2af637 found bug with direct ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 982 diff changeset	351 @staticmethod
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	352 def cosine(p1, p2):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	353 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	354
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	355 @staticmethod
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	356 def distanceNorm2(p1, p2):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	357 return (p1-p2).norm2()
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	358
64 c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	359 @staticmethod
1205 3905b393ade0 kitti loading code seems to be working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1203 diff changeset	360 def plotAll(points, closePolygon = False, options = '', **kwargs):
1203 7b3384a8e409 second version of code loading kitti data, to clean Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1197 diff changeset	361 xCoords = [p.x for p in points]
7b3384a8e409 second version of code loading kitti data, to clean Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1197 diff changeset	362 yCoords = [p.y for p in points]
7b3384a8e409 second version of code loading kitti data, to clean Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1197 diff changeset	363 if closePolygon:
1205 3905b393ade0 kitti loading code seems to be working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1203 diff changeset	364 xCoords.append(xCoords[0])
3905b393ade0 kitti loading code seems to be working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1203 diff changeset	365 yCoords.append(yCoords[0])
1203 7b3384a8e409 second version of code loading kitti data, to clean Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1197 diff changeset	366 plot(xCoords, yCoords, options, **kwargs)
64 c75bcdaed00f added functions for plotting points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 62 diff changeset	367
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	368 def similarOrientation(self, refDirection, cosineThreshold):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	369 '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	370 return Point.cosine(self, refDirection) >= cosineThreshold
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	371
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	372 @staticmethod
484 6464e4f0cc26 integrated Sohail direct computation of TTC (need to add pPET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 464 diff changeset	373 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	374 '''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	375 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	376 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	377 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	378 dv = v1-v2
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	379 dp = p1-p2
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	380 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	381 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	382 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	383
504 a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	384 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	385 if delta >= 0:
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	386 deltaRoot = sqrt(delta)
a40c75f04903 optimized direct time to collision computation and added tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 501 diff changeset	387 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	388 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	389 if ttc1 >= 0 and ttc2 >= 0:
881 8ba82b371eea work on storing PET Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 880 diff changeset	390 return 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	391 elif ttc1 >= 0:
881 8ba82b371eea work on storing PET Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 880 diff changeset	392 return ttc1
531 f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 527 diff changeset	393 elif ttc2 >= 0:
881 8ba82b371eea work on storing PET Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 880 diff changeset	394 return ttc2
531 f012a8ad7a0e corrected bug in Point.timeToCollision that might result in negative TTCs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 527 diff changeset	395 else: # ttc1 < 0 and ttc2 < 0:
881 8ba82b371eea work on storing PET Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 880 diff changeset	396 return None
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	397 else:
881 8ba82b371eea work on storing PET Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 880 diff changeset	398 return None
464 dcc821b98efc integrated and reorganized Sohail s work on exact ttc computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 451 diff changeset	399
1094 c96388c696ac adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1086 diff changeset	400 @staticmethod
1213 3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	401 def ttc_calculate(bbox_info_total_1, bbox_info_total_2):
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	402 """
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	403 :param bbox_1: list: [bbox_1 array 4*2 , v_1_x (float), v_1_y (float)]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	404 :param bbox_2: [bbox_2_x array 4*2, v_2_x (float), v_2_y (float)]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	405 :return: ttc
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	406 """
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	407 from sympy import solve
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	408 def NewFourPoints(bbox, col_time, v_x, v_y):
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	409 return [i + col_time * v_x for i in bbox[0]], [j + col_time * v_y for j in bbox[1]]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	410
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	411 bbox_1, v_1_x, v_1_y = bbox_info_total_1[0], bbox_info_total_1[1], bbox_info_total_1[2]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	412 bbox_2, v_2_x, v_2_y = bbox_info_total_2[0], bbox_info_total_2[1], bbox_info_total_2[2]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	413 x_bbox_1 = bbox_1[:, 0].tolist()
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	414 y_bbox_1 = bbox_1[:, 1].tolist()
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	415 bbox_1 = [x_bbox_1, y_bbox_1]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	416 x_bbox_2 = bbox_2[:, 0].tolist()
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	417 y_bbox_2 = bbox_2[:, 1].tolist()
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	418 bbox_2 = [x_bbox_2, y_bbox_2]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	419 t_total = []
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	420 line = [[0, 1], [1, 2], [2, 3], [3, 0]]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	421 for i in range(4):
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	422 for j in range(4):
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	423 p0_x, p0_y = x_bbox_2[i], y_bbox_2[i]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	424 p1_x, p1_y = x_bbox_1[line[j][0]], y_bbox_1[line[j][0]]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	425 p2_x, p2_y = x_bbox_1[line[j][1]], y_bbox_1[line[j][1]]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	426 f = (p2_x - p1_x) * (p0_y - p1_y) - (p0_x - p1_x) * (p2_y - p1_y)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	427 collision_t = solve(f, t, set=True, dict=True)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	428 if collision_t and collision_t[0][t] >= 0:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	429 collision_t = collision_t[0][t]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	430 print(f'collision_t: {collision_t} >>>>>>>>>')
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	431 x_bbox_1_new, y_bbox_1_new = NewFourPoints(bbox_1, collision_t, v_1_x, v_1_y)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	432 x_bbox_2_new, y_bbox_2_new = NewFourPoints(bbox_2, collision_t, v_2_x, v_2_y)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	433 p0_x, p0_y = x_bbox_2_new[i], y_bbox_2_new[i]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	434 p1_x, p1_y = x_bbox_1_new[line[j][0]], y_bbox_1_new[line[j][0]]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	435 p2_x, p2_y = x_bbox_1_new[line[j][1]], y_bbox_1_new[line[j][1]]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	436 min_x, max_x = min(p1_x, p2_x), max(p1_x, p2_x)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	437 min_y, max_y = min(p1_y, p2_y), max(p1_y, p2_y)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	438 if min_x == max_x:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	439 if min_y <= p0_y <= max_y:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	440 t_total.append(collision_t)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	441
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	442 elif min_y == max_y:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	443 if min_x <= p0_x <= max_x:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	444 t_total.append(collision_t)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	445
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	446 else:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	447 if min_x <= p0_x <= max_x or min_y <= p0_y <= max_y:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	448 t_total.append(collision_t)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	449 x_temps = x_bbox_2
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	450 x_bbox_2 = x_bbox_1
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	451 x_bbox_1 = x_temps
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	452 # print(f'Change bounding box1 and box2 >>>>>>>>>>>>>>>>>>>\n\n\n')
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	453 for i in range(4):
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	454 for j in range(4):
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	455 p0_x, p0_y = x_bbox_2[i], y_bbox_2[i]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	456 p1_x, p1_y = x_bbox_1[line[j][0]], y_bbox_1[line[j][0]]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	457 p2_x, p2_y = x_bbox_1[line[j][1]], y_bbox_1[line[j][1]]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	458 f = (p2_x - p1_x) * (p0_y - p1_y) - (p0_x - p1_x) * (p2_y - p1_y)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	459 collision_t = solve(f, t, set=True, dict=True)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	460 if collision_t and collision_t[0][t] >= 0:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	461 collision_t = collision_t[0][t]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	462 # print(f'collision_t: {collision_t} >>>>>>>>>')
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	463 x_bbox_1_new, y_bbox_1_new = NewFourPoints(bbox_1, collision_t)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	464 x_bbox_2_new, y_bbox_2_new = NewFourPoints(bbox_2, collision_t)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	465 p0_x, p0_y = x_bbox_2_new[i], y_bbox_2_new[i]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	466 p1_x, p1_y = x_bbox_1_new[line[j][0]], y_bbox_1_new[line[j][0]]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	467 p2_x, p2_y = x_bbox_1_new[line[j][1]], y_bbox_1_new[line[j][1]]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	468 min_x, max_x = min(p1_x, p2_x), max(p1_x, p2_x)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	469 min_y, max_y = min(p1_y, p2_y), max(p1_y, p2_y)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	470 if min_x == max_x:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	471 if min_y <= p0_y <= max_y:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	472 t_total.append(collision_t)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	473
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	474 elif min_y == max_y:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	475 if min_x <= p0_x <= max_x:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	476 t_total.append(collision_t)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	477
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	478 else:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	479 if min_x <= p0_x <= max_x or min_y <= p0_y <= max_y:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	480 t_total.append(collision_t)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	481
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	482 if t_total:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	483 collision_t_min = min(t_total)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	484 # print(f'collision_time:')
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	485 # print(collision_t_min)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	486 else:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	487 collision_t_min = 1e7
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	488 # print(f'No collision')
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	489 return collision_t_min
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	490
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	491 @staticmethod
583 6ebfb43e938e added midpoint function (from laurent gauthier) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 582 diff changeset	492 def midPoint(p1, p2):
6ebfb43e938e added midpoint function (from laurent gauthier) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 582 diff changeset	493 '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	494 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	495
1019 5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	496 @staticmethod
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	497 def agg(points, aggFunc = mean):
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	498 return Point(aggFunc([p.x for p in points]), aggFunc([p.y for p in points]))
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	499
1106 799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	500 @staticmethod
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	501 def boundingRectangle(points, v):
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	502 '''Returns the bounding rectangle of the points, aligned on the vector v
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	503 A list of points is returned: front left, front right, rear right, rear left'''
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	504 e1 = v.normalize()
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	505 e2 = e1.orthogonal(False)
1106 799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	506 xCoords = []
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	507 yCoords = []
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	508 for p in points:
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	509 xCoords.append(Point.dot(e1, p))
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	510 yCoords.append(Point.dot(e2, p))
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	511 xmin = min(xCoords)
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	512 xmax = max(xCoords)
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	513 ymin = min(yCoords)
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	514 ymax = max(yCoords)
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	515 frontLeft = Point(xmax, ymax)
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	516 frontRight = Point(xmax, ymin)
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	517 rearLeft = Point(xmin, ymax)
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	518 rearRight = Point(xmin, ymin)
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	519 originalE1 = Point(Point.dot(e1, Point(1,0)),Point.dot(e2, Point(1,0)))
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	520 originalE2 = Point(Point.dot(e1, Point(0,1)),Point.dot(e2, Point(0,1)))
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	521 return [Point(Point.dot(originalE1, p), Point.dot(originalE2, p)) for p in [frontLeft, frontRight, rearRight, rearLeft]]
1109 6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	522
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	523 if shapelyAvailable:
1210 1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	524 def pointsToShapely(points):
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	525 'Returns shapely polygon'
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	526 return Polygon([p.astuple() for p in points])
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	527
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	528 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	529 '''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	530 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	531 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	532 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	533 prepared_polygon = prep(polygon)
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	534 return list(filter(prepared_polygon.contains, points))
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	535
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	536 # Functions for coordinate transformation
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	537 # From Paul St-Aubin's PVA tools
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	538 def prepareAlignments(alignments):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	539 '''Prepares alignments (list of splines, each typically represented as a Trajectory)
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	540 - computes cumulative distances
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	541 - approximates slope singularity by giving some slope roundoff (account for roundoff error)'''
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	542 for alignment in alignments:
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	543 alignment.computeCumulativeDistances()
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	544 p1 = alignment[0]
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	545 for i in range(len(alignment)-1):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	546 p2 = alignment[i+1]
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	547 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	548 p2.x += 0.0000000001
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	549 if(round(p1.y, 10) == round(p2.y, 10)):
1094 c96388c696ac adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1086 diff changeset	550 p2.y += 0.0000000001
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	551 p1 = p2
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	552
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	553 def ppldb2p(qx,qy, p0x,p0y, p1x,p1y):
1094 c96388c696ac adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1086 diff changeset	554 ''' Point-projection (Q) on line defined by 2 points (P0,P1).
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	555 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	556 '''
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	557 if(p0x == p1x and p0y == p1y):
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	558 return None
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	559 try:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	560 #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	561 # 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	562 # p1x += 0.0000000001
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	563 # if(round(p0y, 10) == round(p1y, 10)):
1094 c96388c696ac adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1086 diff changeset	564 # p1y += 0.0000000001
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	565 #make the calculation
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	566 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	567 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	568 except ZeroDivisionError:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	569 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	570 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y))
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	571 import pdb;
6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	572 pdb.set_trace()
6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	573 return Point(X, Y)
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	574
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	575 def getSYfromXY(p, alignments, goodEnoughAlignmentDistance = 0.5):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	576 ''' Snap a point p to its nearest subsegment of it's nearest alignment (from the list alignments).
9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	577 A alignment is a list of points (class Point), most likely a trajectory.
9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	578
659 784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	579 Output:
784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	580 =======
1094 c96388c696ac adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1086 diff changeset	581 [alignment index,
c96388c696ac adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1086 diff changeset	582 subsegment leading point index,
c96388c696ac adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1086 diff changeset	583 snapped point,
c96388c696ac adding functions for simulation, with contribution from student Lionel Nebot-Janvier, lionel.nebot-janvier@polymtl.ca Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1086 diff changeset	584 subsegment distance,
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	585 alignment distance,
659 784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	586 orthogonal point offset]
784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	587
784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	588 or None
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	589 '''
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	590 minOffsetY = float('inf')
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	591 #For each alignment
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	592 for alignmentIdx in range(len(alignments)):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	593 #For each alignment point index
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	594 for alignment_p in range(len(alignments[alignmentIdx])-1):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	595 #Get closest point on alignment
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	596 closestPoint = ppldb2p(p.x,p.y,alignments[alignmentIdx][alignment_p][0],alignments[alignmentIdx][alignment_p][1],alignments[alignmentIdx][alignment_p+1][0],alignments[alignmentIdx][alignment_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	597 if closestPoint is None:
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	598 print('Error: Alignment {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(alignmentIdx, alignment_p))
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	599 return None
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	600 # check if the projected point is in between the current segment of the alignment bounds
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	601 if utils.inBetween(alignments[alignmentIdx][alignment_p][0], alignments[alignmentIdx][alignment_p+1][0], closestPoint.x) and utils.inBetween(alignments[alignmentIdx][alignment_p][1], alignments[alignmentIdx][alignment_p+1][1], closestPoint.y):
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	602 offsetY = Point.distanceNorm2(closestPoint, p)
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	603 if offsetY < minOffsetY:
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	604 minOffsetY = offsetY
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	605 snappedAlignmentIdx = alignmentIdx
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	606 snappedAlignmentLeadingPoint = alignment_p
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	607 snappedPoint = Point(closestPoint.x, closestPoint.y)
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	608 #Jump loop if significantly close
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	609 if offsetY < goodEnoughAlignmentDistance:
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	610 break
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	611
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	612 #Get sub-segment distance
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	613 if minOffsetY != float('inf'):
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	614 subsegmentDistance = Point.distanceNorm2(snappedPoint, alignments[snappedAlignmentIdx][snappedAlignmentLeadingPoint])
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	615 #Get cumulative alignment distance (total segment distance)
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	616 alignmentDistanceS = alignments[snappedAlignmentIdx].getCumulativeDistance(snappedAlignmentLeadingPoint) + subsegmentDistance
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	617 orthogonalAlignmentVector = (alignments[snappedAlignmentIdx][snappedAlignmentLeadingPoint+1]-alignments[snappedAlignmentIdx][snappedAlignmentLeadingPoint]).orthogonal()
578 fe4e9d2b807d finalizing transformcoordinates for each object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 577 diff changeset	618 offsetVector = p-snappedPoint
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	619 if Point.dot(orthogonalAlignmentVector, offsetVector) < 0:
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	620 minOffsetY = -minOffsetY
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	621 return [snappedAlignmentIdx, snappedAlignmentLeadingPoint, snappedPoint, subsegmentDistance, alignmentDistanceS, minOffsetY]
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	622 else:
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	623 print('Offset for point {} is infinite (check with prepareAlignments if some alignment 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	624 return None
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	625
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	626 def getXYfromSY(s, y, alignmentNum, alignments):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	627 ''' Find X,Y coordinate from S,Y data.
568 538fb47b3007 minor modification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	628 if mode = 0 : return Snapped X,Y
538fb47b3007 minor modification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	629 if mode !=0 : return Real X,Y
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	630 '''
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	631 alignment = alignments[alignmentNum]
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	632 i = 1
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	633 while s > alignment.getCumulativeDistance(i) and i < len(alignment):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	634 i += 1
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	635 if i < len(alignment):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	636 d = s - alignment.getCumulativeDistance(i-1) # distance on subsegment
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	637 #Get difference vector and then snap
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	638 dv = alignment[i] - alignment[i-1]
1106 799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	639 normalizedV = dv.normalize()
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	640 #snapped = alignment[i-1] + normalizedV*d # snapped point coordinate along alignment
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	641 # add offset finally
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	642 orthoNormalizedV = normalizedV.orthogonal()
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	643 return alignment[i-1] + normalizedVd + orthoNormalizedVy
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	644 else:
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	645 print('Curvilinear point {} is past the end of the alignement'.format((s, y, alignmentNum)))
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	646 return None
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	647
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	648
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	649 class NormAngle(object):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	650 '''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	651
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	652 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	653 self.norm = norm
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	654 self.angle = angle
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	655
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	656 @staticmethod
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	657 def fromPoint(p):
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	658 norm = p.norm2()
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	659 if norm > 0:
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	660 angle = p.angle()
339 9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	661 else:
9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	662 angle = 0.
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	663 return NormAngle(norm, angle)
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	664
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	665 def __add__(self, other):
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	666 '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	667 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	668
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	669 def getPoint(self):
255 13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	670 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	671
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	672
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	673 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	674 '''Predicts the position in nTimeSteps at constant speed/acceleration'''
939 a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	675 return initialVelocity + initialAcceleration.__mul__(nTimeSteps),initialPosition+initialVelocity.__mul__(nTimeSteps) + initialAcceleration.__mul__(nTimeSteps*20.5)
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	676
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	677 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	678 '''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	679 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	680 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	681 predictedSpeedTheta = speedOrientation+control
928 063d1267585d work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 921 diff changeset	682 if maxSpeed is not None:
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	683 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	684 predictedPosition = position+predictedSpeedTheta.getPoint()
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	685 return predictedPosition, predictedSpeedTheta
245 bd8ab323c198 corrected issue with predictPosiont static method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 244 diff changeset	686
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	687
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	688 class FlowVector(object):
184 d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	689 '''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	690 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	691 def __init__(self, position, velocity):
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	692 '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	693 self.position = position
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	694 self.velocity = velocity
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	695
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	696 def __add__(self, other):
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	697 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	698
939 a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	699 def __mul__(self, alpha):
a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	700 return FlowVector(self.position.__mul__(alpha), self.velocity.__mul__(alpha))
184 d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	701
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	702 def plot(self, options = '', **kwargs):
211 ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	703 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	704 self.position.plot(options+'x', **kwargs)
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	705
184 d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	706 @staticmethod
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	707 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	708 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	709
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	710 def intersection(p1, p2, p3, p4):
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	711 ''' Intersection point (x,y) of the segments [p1, p2] and [p3, p4]
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	712 Returns the intersection point and the ratio of its position along [p1, p2] from p1
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	713
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	714 Based on http://paulbourke.net/geometry/pointlineplane/'''
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	715 dp12 = p2-p1
571 a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	716 dp34 = p4-p3
a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	717 #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	718 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	719 if det == 0.:
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	720 return None, None
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	721 else:
571 a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	722 ua = (dp34.x(p1.y-p3.y)-dp34.y(p1.x-p3.x))/det
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	723 return p1+dp12.__mul__(ua), ua
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	724
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	725 # def intersection(p1, p2, dp1, dp2):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	726 # '''Returns the intersection point between the two lines
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	727 # 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	728 # from numpy import matrix
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	729 # from numpy.linalg import linalg
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	730 # A = matrix([[dp1.y, -dp1.x],
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	731 # [dp2.y, -dp2.x]])
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	732 # 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	733 # [dp2.yp2.x-dp2.xp2.y]])
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	734
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	735 # if linalg.det(A) == 0:
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	736 # return None
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	737 # else:
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	738 # intersection = linalg.solve(A,B)
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	739 # 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	740
152 74b1fc68d4df re-organized code to avoid cyclic python module dependencies Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 148 diff changeset	741 def segmentIntersection(p1, p2, p3, p4):
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	742 '''Returns the intersecting point (and ratio along [p1, p2]) of the segments [p1, p2] and [p3, p4], None otherwise'''
152 74b1fc68d4df re-organized code to avoid cyclic python module dependencies Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 148 diff changeset	743
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	744 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()):
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	745 return None, None
152 74b1fc68d4df re-organized code to avoid cyclic python module dependencies Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 148 diff changeset	746 else:
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	747 inter, ratio = intersection(p1, p2, p3, p4)
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	748 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	749 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	750 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	751 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	752 and utils.inBetween(p3.y, p4.y, inter.y)):
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	753 return inter, ratio
359 619ae9a9a788 implemented prediction method at constant velocity with direct intersection computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 357 diff changeset	754 else:
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	755 return None, None
258 d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	756
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	757 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	758 '''Indicates if the line going through p1 and p2 intersects inside p3, p4'''
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	759 inter, ratio = 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	760 if inter is not None and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p3.y, p4.y, inter.y):
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	761 return inter, ratio
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	762 else:
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	763 return None, None
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	764
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	765 def segmentOrientationCrossing(p1, p2, p3, p4):
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	766 '''Returns the direction of the crossing, assuming there is a crossing: True means right (p3) to left (p4) (along the orthogonal vector to [p1, p2] (positive trigonometric orientation), False the other way'''
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	767 dp12 = p2-p1
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	768 ortho = dp12.orthogonal(False)
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	769 return Point.dot(ortho, p4-p3) > 0
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	770
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	771 class Trajectory(object):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	772 '''Class for trajectories: temporal sequence of positions
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	773
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	774 The class is iterable'''
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	775
79 5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	776 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	777 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	778 self.positions = positions
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	779 else:
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	780 self.positions = [[],[]]
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	781
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	782 @staticmethod
555 f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	783 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	784 t = Trajectory()
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	785 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	786 t.addPosition(p0)
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	787 for i in range(nPoints-1):
555 f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	788 p0 += v
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	789 t.addPosition(p0)
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	790 return t
555 f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	791
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	792 @staticmethod
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	793 def load(line1, line2):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	794 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	795 [float(n) for n in line2.split(' ')]])
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	796
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	797 @staticmethod
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	798 def fromPointList(points):
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	799 t = Trajectory()
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	800 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	801 for p in points:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	802 t.addPositionXY(p[0],p[1])
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	803 else:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	804 for p in points:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	805 t.addPosition(p)
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	806 return t
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	807
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	808 def __len__(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	809 return len(self.positions[0])
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	810
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	811 def length(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	812 return self.__len__()
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	813
582 7e1ae4d97f1a corrected bug for curvilinear trajectory with only one position and differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 581 diff changeset	814 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	815 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	816
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	817 def __getitem__(self, i):
1209 2064e52019db work on TTC for BEV Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1207 diff changeset	818 if isinstance(i, int):# or issubdtype(i, npinteger):
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	819 return Point(self.positions[0][i], self.positions[1][i])
918 3a06007a4bb7 modularized save trajectories, added slice to Trajectory, etc Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 904 diff changeset	820 elif isinstance(i, slice):
3a06007a4bb7 modularized save trajectories, added slice to Trajectory, etc Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 904 diff changeset	821 return Trajectory([self.positions[0][i],self.positions[1][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	822 else:
978 184f1dd307f9 corrected print and exception statements for Python 3 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 959 diff changeset	823 raise TypeError("Invalid argument type.")
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	824
16 9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	825 def __str__(self):
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	826 return ' '.join([self.__getitem__(i).__str__() for i in range(self.length())])
16 9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	827
69 cc192d0450b3 added full support for two implementations of indicators, with tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 68 diff changeset	828 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	829 return self.__str__()
23 5f2921ad4f7e made Trajectory indexable and timeinterval iterable Nicolas Saunier <nico@confins.net> parents: 22 diff changeset	830
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	831 def __iter__(self):
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	832 self.iterInstantNum = 0
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	833 return self
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	834
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	835 def __next__(self):
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	836 if self.iterInstantNum >= self.length():
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	837 raise StopIteration
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	838 else:
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	839 self.iterInstantNum += 1
23 5f2921ad4f7e made Trajectory indexable and timeinterval iterable Nicolas Saunier <nico@confins.net> parents: 22 diff changeset	840 return self[self.iterInstantNum-1]
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	841
776 84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	842 def __eq__(self, other):
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	843 if self.length() == other.length():
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	844 result = True
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	845 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	846 result = result and (p == po)
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	847 return result
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	848 else:
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	849 return False
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	850
1122 58efbe18f102 slight modification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1117 diff changeset	851 def append(self,other):
58efbe18f102 slight modification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1117 diff changeset	852 '''adds positions of other to the trajectory (in-place modification)'''
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	853 for i in range(2):
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	854 self.positions[i] += other.positions[i]
1122 58efbe18f102 slight modification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1117 diff changeset	855
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	856 def setPositionXY(self, i, x, y):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	857 if i < self.__len__():
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	858 self.positions[0][i] = x
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	859 self.positions[1][i] = y
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	860
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	861 def setPosition(self, i, p):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	862 self.setPositionXY(i, p.x, p.y)
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	863
71 45e958ccd9bd added new addPosition method to Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 69 diff changeset	864 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	865 self.positions[0].append(x)
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	866 self.positions[1].append(y)
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	867
71 45e958ccd9bd added new addPosition method to Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 69 diff changeset	868 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	869 self.addPositionXY(p.x, p.y)
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	870
343 74e437ab5f11 first version of indicator loading code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 339 diff changeset	871 def duplicateLastPosition(self):
74e437ab5f11 first version of indicator loading code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 339 diff changeset	872 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	873 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	874
182 d3f6de6c3918 added drawing functialities, in particular on aerial image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 152 diff changeset	875 @staticmethod
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	876 def _plot(positions, options = '', withOrigin = False, lastCoordinate = None, timeStep = 1, objNum = None, **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	877 if lastCoordinate is None:
270 05c9b0cb8202 updates for drawing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 268 diff changeset	878 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	879 elif 0 <= lastCoordinate <= len(positions[0]):
270 05c9b0cb8202 updates for drawing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 268 diff changeset	880 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	881 if withOrigin:
211 ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	882 plot([positions[0][0]], [positions[1][0]], 'ro', **kwargs)
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	883 if objNum is not None:
07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	884 text(positions[0][0], positions[1][0], '{}'.format(objNum))
933 8ac7f61c6e4f major rework of homography calibration, no in ideal points if correcting for distortion Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 929 diff changeset	885
8ac7f61c6e4f major rework of homography calibration, no in ideal points if correcting for distortion Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 929 diff changeset	886 def homographyProject(self, homography):
8ac7f61c6e4f major rework of homography calibration, no in ideal points if correcting for distortion Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 929 diff changeset	887 return Trajectory(cvutils.homographyProject(array(self.positions), homography).tolist())
8ac7f61c6e4f major rework of homography calibration, no in ideal points if correcting for distortion Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 929 diff changeset	888
8ac7f61c6e4f major rework of homography calibration, no in ideal points if correcting for distortion Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 929 diff changeset	889 def newCameraProject(self, newCameraMatrix):
8ac7f61c6e4f major rework of homography calibration, no in ideal points if correcting for distortion Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 929 diff changeset	890 return Trajectory(cvutils.newCameraProject(array(self.positions), newCameraMatrix).tolist())
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	891
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	892 def plot(self, options = '', withOrigin = False, timeStep = 1, objNum = None, **kwargs):
07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	893 Trajectory._plot(self.positions, options, withOrigin, None, timeStep, objNum, **kwargs)
182 d3f6de6c3918 added drawing functialities, in particular on aerial image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 152 diff changeset	894
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	895 def plotAt(self, lastCoordinate, options = '', withOrigin = False, timeStep = 1, objNum = None, **kwargs):
07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	896 Trajectory._plot(self.positions, options, withOrigin, lastCoordinate, timeStep, objNum, **kwargs)
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	897
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	898 def plotOnWorldImage(self, nPixelsPerUnitDistance, options = '', withOrigin = False, timeStep = 1, objNum = None, **kwargs):
182 d3f6de6c3918 added drawing functialities, in particular on aerial image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 152 diff changeset	899 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	900 [x*nPixelsPerUnitDistance for x in self.positions[1]]]
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	901 Trajectory._plot(imgPositions, options, withOrigin, None, timeStep, objNum, **kwargs)
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	902
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	903 def getXCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	904 return self.positions[0]
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	905
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	906 def getYCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	907 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	908
a5ef9e40688e makes use of matplotlib function to test if point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 91 diff changeset	909 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	910 return array(self.positions)
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	911
14 e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	912 def xBounds(self):
e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	913 # 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	914 return Interval(min(self.getXCoordinates()), max(self.getXCoordinates()))
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	915
14 e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	916 def yBounds(self):
e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	917 # 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	918 return Interval(min(self.getYCoordinates()), max(self.getYCoordinates()))
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	919
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	920 def add(self, traj2):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	921 '''Returns a new trajectory of the same length'''
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	922 if self.length() != traj2.length():
978 184f1dd307f9 corrected print and exception statements for Python 3 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 959 diff changeset	923 print('Trajectories of different lengths')
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	924 return None
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	925 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	926 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	927 [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	928
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	929 def subtract(self, traj2):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	930 '''Returns a new trajectory of the same length'''
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	931 if self.length() != traj2.length():
978 184f1dd307f9 corrected print and exception statements for Python 3 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 959 diff changeset	932 print('Trajectories of different lengths')
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	933 return None
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	934 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	935 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	936 [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	937
939 a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	938 def __mul__(self, alpha):
590 0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	939 '''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	940 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	941 [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	942
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	943 def differentiate(self, doubleLastPosition = False):
339 9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	944 diff = Trajectory()
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	945 for i in range(1, self.length()):
339 9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	946 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	947 if doubleLastPosition:
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	948 diff.addPosition(diff[-1])
339 9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	949 return diff
9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	950
1196 d5566af60a69 correcting bug with savgol filter, renaming the functions to proper names filterSG and getAccelerationsSG Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1195 diff changeset	951 def filterSG(self, window_length, polyorder, deriv=0, delta=1.0, axis=-1, mode='nearest', cval=0.0, nInstantsIgnoredAtEnds = 2):
d5566af60a69 correcting bug with savgol filter, renaming the functions to proper names filterSG and getAccelerationsSG Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1195 diff changeset	952 '''Filters the trajectory using the Savitsky Golay filter
d5566af60a69 correcting bug with savgol filter, renaming the functions to proper names filterSG and getAccelerationsSG Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1195 diff changeset	953 if deriv = 1, the method differentiates
d5566af60a69 correcting bug with savgol filter, renaming the functions to proper names filterSG and getAccelerationsSG Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1195 diff changeset	954 Warning: high order polynomials yield artefacts
761 15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	955
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	956 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	957 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	958 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	959 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	960 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	961 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.
877 d1ff6917d082 added savitzky golay filter for accelerations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 869 diff changeset	962 cval : Value to fill past the edges of the input if mode is constant. Default is 0.0.
761 15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	963
877 d1ff6917d082 added savitzky golay filter for accelerations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 869 diff changeset	964 https://docs.scipy.org/doc/scipy/reference/generated/scipy.signal.savgol_filter.html#scipy.signal.savgol_filter'''
1193 d324305c1240 solving issues with extreme values when filtering using savitsky golay Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1190 diff changeset	965 filtered = savgol_filter(self.positions, window_length, polyorder, deriv, delta, axis, mode, cval)
1196 d5566af60a69 correcting bug with savgol filter, renaming the functions to proper names filterSG and getAccelerationsSG Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1195 diff changeset	966 length = self.length()
1181 b3b1a5dfa17c correct bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1180 diff changeset	967 if nInstantsIgnoredAtEnds >=1:
1196 d5566af60a69 correcting bug with savgol filter, renaming the functions to proper names filterSG and getAccelerationsSG Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1195 diff changeset	968 if nInstantsIgnoredAtEnds >= length/2:
d5566af60a69 correcting bug with savgol filter, renaming the functions to proper names filterSG and getAccelerationsSG Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1195 diff changeset	969 n = int(round(length/2))-1
1195 27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	970 else:
27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	971 n = nInstantsIgnoredAtEnds
27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	972 filtered = filtered[:,n:-n]
1193 d324305c1240 solving issues with extreme values when filtering using savitsky golay Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1190 diff changeset	973 return Trajectory(filtered.tolist())
761 15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	974
16 9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	975 def norm(self):
9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	976 '''Returns the list of the norms at each instant'''
248 571ba5ed22e2 added utils for bus processing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 245 diff changeset	977 return hypot(self.positions[0], self.positions[1])
14 e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	978
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	979 def computeCumulativeDistances(self):
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	980 '''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	981 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	982 self.distances = []
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	983 self.cumulativeDistances = [0.]
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	984 p1 = self[0]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	985 cumulativeDistance = 0.
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	986 for i in range(self.length()-1):
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	987 p2 = self[i+1]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	988 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	989 cumulativeDistance += self.distances[-1]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	990 self.cumulativeDistances.append(cumulativeDistance)
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	991 p1 = p2
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	992
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	993 def getDistance(self,i):
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	994 '''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	995 if i < self.length()-1:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	996 return self.distances[i]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	997 else:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	998 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	999
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	1000 def getCumulativeDistance(self, i):
1115 cef7aa2f9931 minor shorthand method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1114 diff changeset	1001 '''Returns the cumulative distance between the beginning and point i'''
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	1002 if i < self.length():
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	1003 return self.cumulativeDistances[i]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	1004 else:
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	1005 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	1006
1115 cef7aa2f9931 minor shorthand method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1114 diff changeset	1007 def getTotalDistance(self):
cef7aa2f9931 minor shorthand method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1114 diff changeset	1008 '''Returns the total distance (shorthand for cumulative distance [-1]'''
cef7aa2f9931 minor shorthand method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1114 diff changeset	1009 return self.getCumulativeDistance(-1)
cef7aa2f9931 minor shorthand method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1114 diff changeset	1010
762 d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1011 def getMaxDistance(self, metric):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1012 'Returns the maximum distance between points in the trajectory'
762 d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1013 positions = self.getPositions().asArray().T
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1014 return cdist(positions, positions, metric = metric).max()
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1015
937 b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1016 def getClosestPoint(self, p1, maxDist2 = None):
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1017 '''Returns the instant of the closest position in trajectory to p1 (and the point)
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1018 if maxDist is not None, will check the distance is smaller
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1019 TODO: could use cdist for different metrics'''
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1020 distances2 = []
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1021 minDist2 = float('inf')
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1022 i = -1
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1023 for p2 in self:
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1024 distances2.append(Point.distanceNorm2(p1, p2))
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1025 if distances2[-1] < minDist2:
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1026 minDist2 = distances2[-1]
939 a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	1027 i = len(distances2)-1
a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	1028 if maxDist2 is not None and minDist2 < maxDist2:
a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	1029 return None
a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	1030 else:
937 b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1031 return i
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1032
381 387cc0142211 script to replay event annotations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 379 diff changeset	1033 def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1034 '''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	1035 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	1036 count = 0
383 0ce2210790b1 fixed stupid naming bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 381 diff changeset	1037 lengthThreshold = float(self.length())*minProportion
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1038 for p in self:
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1039 if p.similarOrientation(refDirection, cosineThreshold):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1040 count += 1
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1041 return count >= lengthThreshold
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1042
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	1043 def wiggliness(self):
762 d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1044 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	1045 if straightDistance > 0:
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1046 return self.getCumulativeDistance(self.length()-1)/float(straightDistance)
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1047 else:
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1048 return None
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	1049
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1050 def getIntersections(self, p1, p2, computeOrientations = False):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1051 '''Returns a list of the indices at which the trajectory
9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1052 intersects with the segment of extremities p1 and p2
771 bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	1053 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	1054 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	1055 intersections = []
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1056 rightToLeftOrientations = []
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1057
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	1058 for i in range(self.length()-1):
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1059 q1=self.__getitem__(i)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1060 q2=self.__getitem__(i+1)
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1061 p, ratio = segmentIntersection(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	1062 if p is not None:
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1063 # if q1.x != q2.x:
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1064 # ratio = (p.x-q1.x)/(q2.x-q1.x)
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1065 # elif q1.y != q2.y:
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1066 # ratio = (p.y-q1.y)/(q2.y-q1.y)
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1067 # else:
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1068 # ratio = 0
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1069 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	1070 intersections.append(p)
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1071 if computeOrientations:
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1072 rightToLeftOrientations.append(segmentOrientationCrossing(p1, p2, q1, q2))
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1073 return indices, intersections, rightToLeftOrientations
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1074
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	1075 def getLineIntersections(self, p1, p2):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1076 '''Returns a list of the indices at which the trajectory
9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1077 intersects with the line going through p1 and p2
771 bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	1078 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	1079 indices = []
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	1080 intersections = []
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1081
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	1082 for i in range(self.length()-1):
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	1083 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	1084 q2=self.__getitem__(i+1)
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1085 p, ratio = segmentLineIntersection(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	1086 if p is not None:
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1087 # if q1.x != q2.x:
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1088 # ratio = (p.x-q1.x)/(q2.x-q1.x)
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1089 # elif q1.y != q2.y:
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1090 # ratio = (p.y-q1.y)/(q2.y-q1.y)
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1091 # else:
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1092 # ratio = 0
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	1093 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	1094 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	1095 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	1096
1109 6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1097 def subTrajectoryInInterval(self, inter):
622 dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	1098 '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	1099 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	1100 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	1101 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	1102 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1103 return None
762 d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1104
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1105 def subSample(self, step):
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1106 'Returns the positions very step'
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1107 return Trajectory([self.positions[0][::step],
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1108 self.positions[1][::step]])
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1109
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	1110 if shapelyAvailable:
863 a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1111 def getInstantsInPolygon(self, polygon):
a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1112 '''Returns the list of instants at which the trajectory is in the polygon'''
a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1113 instants = []
a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1114 n = self.length()
a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1115 for t, x, y in zip(range(n), self.positions[0], self.positions[1]):
a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1116 if polygon.contains(shapelyPoint(x, y)):
a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1117 instants.append(t)
a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1118 return instants
a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1119
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	1120 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	1121 '''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	1122 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	1123
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1124 t2 is another trajectory (could be velocities)
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	1125 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	1126 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	1127 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	1128 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	1129 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	1130 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	1131 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	1132 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	1133 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	1134 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	1135 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	1136 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	1137 return traj, traj2
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1138
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1139 def proportionInPolygon(self, polygon, minProportion = 0.5):
863 a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1140 instants = self.getInstantsInPolygon(polygon)
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1141 lengthThreshold = float(self.length())*minProportion
863 a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1142 return len(instants) >= lengthThreshold
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1143 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	1144 def getTrajectoryInPolygon(self, polygon, t2 = None):
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1145 '''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	1146 (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	1147 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	1148 inPolygon = []
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1149 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	1150 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	1151 if inPolygon[-1]:
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1152 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	1153 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	1154 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	1155 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	1156 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	1157 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	1158 return traj, traj2
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1159
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1160 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	1161 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	1162 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	1163 return sum(inPolygon) >= lengthThreshold
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1164
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	1165 @staticmethod
369 027e254f0b53 lcss subclass for indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 368 diff changeset	1166 def lcss(t1, t2, lcss):
027e254f0b53 lcss subclass for indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 368 diff changeset	1167 return lcss.compute(t1, t2)
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	1168
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1169 class CurvilinearTrajectory(Trajectory):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1170 '''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	1171 longitudinal coordinate is stored as first coordinate (exterior name S)
1085 7853106677b7 added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1081 diff changeset	1172 lateral coordinate is stored as second coordinate
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1173 the third "lane" coordinate is for an alignment id,
9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1174 whether explicit for a list/dict of alignments,
1085 7853106677b7 added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1081 diff changeset	1175 or implicit for a road with lane numbers'''
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1176
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	1177 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	1178 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	1179 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	1180 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	1181 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	1182 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	1183 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	1184 if lanes is None or len(lanes) != self.length():
1085 7853106677b7 added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1081 diff changeset	1185 self.lanes = [None]*int(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	1186 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	1187 self.lanes = lanes
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1188
1085 7853106677b7 added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1081 diff changeset	1189 @staticmethod
7853106677b7 added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1081 diff changeset	1190 def generate(s, v, nPoints, lane, y = 0):
7853106677b7 added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1081 diff changeset	1191 '''s is initial position, v is velocity
7853106677b7 added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1081 diff changeset	1192 0 in lateral coordinate by default
7853106677b7 added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1081 diff changeset	1193 TODO 2D velocity for lane change?'''
7853106677b7 added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1081 diff changeset	1194 S = [s]
7853106677b7 added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1081 diff changeset	1195 for i in range(nPoints-1):
7853106677b7 added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1081 diff changeset	1196 S.append(S[-1]+v)
7853106677b7 added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1081 diff changeset	1197 Y = [y]*nPoints
7853106677b7 added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1081 diff changeset	1198 lanes = [lane]*nPoints
7853106677b7 added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1081 diff changeset	1199 return CurvilinearTrajectory(S, Y, lanes)
7853106677b7 added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1081 diff changeset	1200
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1201 @staticmethod
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1202 def fromTrajectoryProjection(t, alignments, halfWidth = 3):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1203 ''' Add, for every object position, the class 'moving.CurvilinearTrajectory()'
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1204 (curvilinearPositions instance) which holds information about the
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1205 curvilinear coordinates using alignment metadata.
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1206 From Paul St-Aubin's PVA tools
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1207 ======
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1208
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1209 Input:
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1210 ======
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1211 alignments = a list of alignments, where each alignment is a list of
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1212 points (class Point).
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1213 halfWidth = moving average window (in points) in which to smooth
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1214 lane changes. As per tools_math.cat_mvgavg(), this term
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1215 is a search radius around the center of the window.
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1216
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1217 '''
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1218 curvilinearPositions = CurvilinearTrajectory()
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1219
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1220 #For each point
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1221 for i in range(int(t.length())):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1222 result = getSYfromXY(t[i], alignments)
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1223
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1224 # Error handling
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1225 if(result is None):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1226 print('Warning: trajectory at point {} {} has alignment errors (alignment snapping)\nCurvilinear trajectory could not be computed'.format(i, t[i]))
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1227 else:
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1228 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1229 curvilinearPositions.addPositionSYL(S, Y, align)
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1230
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1231 ## Go back through points and correct lane
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1232 #Run through objects looking for outlier point
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1233 smoothed_lanes = utils.filterCategoricalMovingWindow(curvilinearPositions.getLanes(), halfWidth)
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1234 ## Recalculate projected point to new lane
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1235 lanes = curvilinearPositions.getLanes()
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1236 if(lanes != smoothed_lanes):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1237 for i in range(len(lanes)):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1238 if(lanes[i] != smoothed_lanes[i]):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1239 result = getSYfromXY(t[i],[alignments[smoothed_lanes[i]]])
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1240
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1241 # Error handling
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1242 if(result is None):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1243 ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment.
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1244 print(' Warning: trajectory at point {} {} has alignment errors during trajectory smoothing and will not be corrected.'.format(i, t[i]))
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1245 else:
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1246 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1247 curvilinearPositions.setPosition(i, S, Y, align)
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1248 return curvilinearPositions
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1249
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1250 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	1251 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	1252 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	1253 else:
978 184f1dd307f9 corrected print and exception statements for Python 3 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 959 diff changeset	1254 raise TypeError("Invalid argument type.")
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	1255 #elif isinstance( key, slice ):
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1256
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1257 def getSCoordinates(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1258 return self.getXCoordinates()
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1259
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1260 def getLanes(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1261 return self.lanes
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1262
1097 b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1263 def getSCoordAt(self, i):
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1264 return self.positions[0][i]
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1265
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1266 def getYCoordAt(self, i):
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1267 return self.positions[1][i]
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1268
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1269 def getLaneAt(self, i):
1098 469e36eea158 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1097 diff changeset	1270 return self.lanes[i]
1097 b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1271
1109 6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1272 def subTrajectoryInInterval(self, inter):
6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1273 'Returns all curvilinear positions between index inter.first and index.last (included)'
6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1274 if inter.first >=0 and inter.last<= self.length():
6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1275 return CurvilinearTrajectory(self.positions[0][inter.first:inter.last+1],
6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1276 self.positions[1][inter.first:inter.last+1],
6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1277 self.lanes[inter.first:inter.last+1])
6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1278 else:
6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1279 return None
6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1280
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1281 def append(self, other):
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1282 Trajectory.append(self, other)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1283 self.lanes.append(other.getLanes())
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1284
1085 7853106677b7 added generate static function for CurvilinearTrajectory and modified how to create them with None list in lanes Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1081 diff changeset	1285 def addPositionSYL(self, s, y, lane = None):
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1286 self.addPositionXY(s,y)
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1287 self.lanes.append(lane)
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1288
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	1289 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	1290 '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	1291 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	1292
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1293 def duplicateLastPosition(self):
6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1294 super(CurvilinearTrajectory, self).duplicateLastPosition()
6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1295 self.lanes.append(self.lanes[-1])
6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1296
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1297 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	1298 self.setPositionXY(i, s, y)
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1299 if i < self.__len__():
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1300 self.lanes[i] = lane
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1301
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	1302 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	1303 diff = CurvilinearTrajectory()
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	1304 p1 = self[0]
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	1305 for i in range(1, self.length()):
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	1306 p2 = self[i]
1098 469e36eea158 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1097 diff changeset	1307 if p2[2] == p1[2]:
469e36eea158 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1097 diff changeset	1308 laneChange = None
469e36eea158 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1097 diff changeset	1309 else:
469e36eea158 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1097 diff changeset	1310 laneChange = (p1[2], p2[2])
469e36eea158 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1097 diff changeset	1311 diff.addPositionSYL(p2[0]-p1[0], p2[1]-p1[1], laneChange)
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	1312 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	1313 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	1314 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	1315 return diff
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	1316
451 cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	1317 def getIntersections(self, S1, lane = None):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1318 '''Returns a list of the indices at which the trajectory
451 cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	1319 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	1320 (in provided lane if lane is not None)
771 bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	1321 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	1322 indices = []
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	1323 for i in range(self.length()-1):
451 cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	1324 q1=self.__getitem__(i)
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	1325 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	1326 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	1327 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	1328 return indices
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	1329
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1330 ##################
1210 1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1331 # Geometry
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1332 ##################
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1333
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1334 class Geometry(object):
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1335 '''Generic class for outline and size of object '''
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1336
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1337
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1338 class CarGeometry(Geometry):
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1339 '''Default car geometry as rectangle '''
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1340
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1341 ##################
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1342 # Moving Objects
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1343 ##################
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1344
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	1345 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	1346 'car',
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1347 '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	1348 'motorcycle',
a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	1349 'bicycle',
185 c06379f25ab8 utilities for user types Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 184 diff changeset	1350 'bus',
1125 b358bed29ab4 updates and bugs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1117 diff changeset	1351 'truck',
b358bed29ab4 updates and bugs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1117 diff changeset	1352 'automated']
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1353
185 c06379f25ab8 utilities for user types Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 184 diff changeset	1354 userType2Num = utils.inverseEnumeration(userTypeNames)
c06379f25ab8 utilities for user types Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 184 diff changeset	1355
959 4f32d82ca390 corrected error due to change in Hog (scikit image) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 956 diff changeset	1356 class CarClassifier:
4f32d82ca390 corrected error due to change in Hog (scikit image) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 956 diff changeset	1357 def predict(self, hog):
4f32d82ca390 corrected error due to change in Hog (scikit image) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 956 diff changeset	1358 return userType2Num['car']
4f32d82ca390 corrected error due to change in Hog (scikit image) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 956 diff changeset	1359 carClassifier = CarClassifier()
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1360
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	1361 class MovingObject(STObject, VideoFilenameAddable):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1362 '''Class for moving objects: a spatio-temporal object
9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1363 with a trajectory and a geometry (constant volume over time)
588 c5406edbcf12 added loading ground truth annotations (ground truth) from polytrack format Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 587 diff changeset	1364 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	1365 '''
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	1366
1203 7b3384a8e409 second version of code loading kitti data, to clean Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1197 diff changeset	1367 def __init__(self, num = None, timeInterval = None, positions = None, velocities = None, geometry = None, userType = userType2Num['unknown'], nObjects = None, features = None, initCurvilinear = False):
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	1368 super(MovingObject, self).__init__(num, timeInterval)
1097 b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1369 if initCurvilinear:
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1370 self.curvilinearPositions = positions
1098 469e36eea158 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1097 diff changeset	1371 self.curvilinearVelocities = velocities # third component is (previousAlignmentIdx, newAlignmentIdx) or None if no change
1097 b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1372 else:
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1373 self.positions = positions
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1374 self.velocities = velocities
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	1375 self.geometry = geometry
54 c354d41ef7cd corrected code for usertype in movingobject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 49 diff changeset	1376 self.userType = userType
1041 fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1377 self.setNObjects(nObjects) # a feature has None for nObjects
1203 7b3384a8e409 second version of code loading kitti data, to clean Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1197 diff changeset	1378 self.features = features
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	1379 # compute bounding polygon from trajectory
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1380
1075 67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1381 @staticmethod
1079 845d694af7b8 reversed minor bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1077 diff changeset	1382 def croppedTimeInterval(obj, value, after = True):
1075 67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1383 newTimeInterval = TimeInterval(obj.getFirstInstant(), min(value, obj.getLastInstant())) if after else TimeInterval(max(obj.getFirstInstant(), value), obj.getLastInstant())
67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1384 if obj.positions is not None :
1136 30171d4fd3df moving.py [Issue #22 from Etienne Beauchamp In annotationTool Correction] Tertuis Ouédraogo <tertuis95@gmail.com> parents: 1135 diff changeset	1385 newPositions = obj.positions[slice(newTimeInterval.first - obj.getLastInstant(), newTimeInterval.last + 1 - obj.getLastInstant())]
1075 67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1386 else:
67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1387 newPositions = None
67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1388 if obj.velocities is not None :
1136 30171d4fd3df moving.py [Issue #22 from Etienne Beauchamp In annotationTool Correction] Tertuis Ouédraogo <tertuis95@gmail.com> parents: 1135 diff changeset	1389 newVelocities = obj.velocities[slice(newTimeInterval.first - obj.getLastInstant(), newTimeInterval.last + 1 - obj.getLastInstant())]
1075 67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1390 else:
67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1391 newVelocities = None
1079 845d694af7b8 reversed minor bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1077 diff changeset	1392 if obj.hasFeatures():
845d694af7b8 reversed minor bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1077 diff changeset	1393 newFeatures = [f.croppedTimeInterval(value, after) for f in obj.features]
1075 67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1394 else:
67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1395 newFeatures = None
67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1396 res = MovingObject(obj.getNum(), newTimeInterval, newPositions, newVelocities, obj.geometry, obj.userType, obj.nObjects)
67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1397 res.features = newFeatures
67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1398 res.featureNumbers = obj.featureNumbers
1079 845d694af7b8 reversed minor bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1077 diff changeset	1399 #if hasattr(obj, 'projectedPositions'):
845d694af7b8 reversed minor bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1077 diff changeset	1400 # res.projectedPositions = obj.projectedPositions[slice(newTimeInterval.first, newTimeInterval.last+1)]
1075 67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1401 return res
67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1402
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	1403
555 f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	1404 @staticmethod
1019 5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1405 def aggregateTrajectories(features, interval = None, aggFunc = mean):
995 349cd5e73f79 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 994 diff changeset	1406 'Computes the aggregate trajectory from list of MovingObject features'
1019 5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1407 positions = Trajectory()
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1408 velocities = Trajectory()
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1409 if interval is None:
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1410 inter = TimeInterval.unionIntervals([f.getTimeInterval() for f in features])
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1411 else:
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1412 inter = interval
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1413 for t in inter:
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1414 points = []
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1415 vels = []
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1416 for f in features:
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1417 if f.existsAtInstant(t):
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1418 points.append(f.getPositionAtInstant(t))
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1419 vels.append(f.getVelocityAtInstant(t))
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1420 positions.addPosition(Point.agg(points, aggFunc))
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1421 velocities.addPosition(Point.agg(vels, aggFunc))
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1422 return inter, positions, velocities
995 349cd5e73f79 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 994 diff changeset	1423
349cd5e73f79 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 994 diff changeset	1424 @staticmethod
918 3a06007a4bb7 modularized save trajectories, added slice to Trajectory, etc Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 904 diff changeset	1425 def generate(num, p, v, timeInterval):
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1426 nPoints = int(timeInterval.length())
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1427 positions = Trajectory.generate(p, v, nPoints)
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1428 return MovingObject(num = num, timeInterval = timeInterval, positions = positions, velocities = Trajectory([[v.x]nPoints, [v.y]nPoints]))
555 f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	1429
1019 5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1430 def updatePositions(self):
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1431 inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval())
555 f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	1432
622 dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	1433 @staticmethod
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1434 def concatenate(obj1, obj2, num = None, newFeatureNum = None):
1018 d7afc59f6966 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1016 diff changeset	1435 '''Concatenates two objects, whether overlapping temporally or not
d7afc59f6966 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1016 diff changeset	1436
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1437 Positions will be recomputed if features are merged
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1438 Otherwise, only featureNumbers and/or features will be merged'''
997 4f3387a242a1 updated utils to python 3 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 996 diff changeset	1439 if num is None:
4f3387a242a1 updated utils to python 3 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 996 diff changeset	1440 newNum = obj1.getNum()
4f3387a242a1 updated utils to python 3 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 996 diff changeset	1441 else:
4f3387a242a1 updated utils to python 3 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 996 diff changeset	1442 newNum = num
623 ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1443 commonTimeInterval = obj1.commonTimeInterval(obj2)
1180 dc28364f34b9 minor cleanup Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1179 diff changeset	1444 if commonTimeInterval.empty(): # and emptyInterval.length() >= 2: not needed 2 to include situations successive positions, but issues with missing last velocity and reloading
dc28364f34b9 minor cleanup Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1179 diff changeset	1445 emptyInterval = TimeInterval(min(obj1.getLastInstant(),obj2.getLastInstant()), max(obj1.getFirstInstant(),obj2.getFirstInstant()))
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1446 if newFeatureNum is None:
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1447 print('Not merging objects {} and {}, missing new feature number'.format(obj1.getNum(),obj2.getNum()))
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1448 return None, None
1018 d7afc59f6966 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1016 diff changeset	1449 else:
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1450 if obj1.existsAtInstant(emptyInterval.last):
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1451 firstObject = obj2
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1452 secondObject = obj1
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1453 else:
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1454 firstObject = obj1
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1455 secondObject = obj2
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1456 v = (secondObject.getPositionAtInstant(emptyInterval.last)-firstObject.getPositionAtInstant(emptyInterval.first)).divide(emptyInterval.length()-1)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1457 positions = copy.deepcopy(firstObject.getPositions())
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1458 velocities = copy.deepcopy(firstObject.getPositions())
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1459 featurePositions = Trajectory()
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1460 featureVelocities = Trajectory()
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1461 p = firstObject.getPositionAtInstant(emptyInterval.first)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1462 # init new feature with position at last instant of 1st obj
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1463 featurePositions.addPosition(p)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1464 featureVelocities.addPosition(v)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1465 for t in range(emptyInterval.first+1, emptyInterval.last):
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1466 p=p+v
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1467 positions.addPosition(p)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1468 velocities.addPosition(v)
1134 4b2a55d570c1 resolve issue for short features when merging objects manually Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1133 diff changeset	1469 featurePositions.addPosition(p)
4b2a55d570c1 resolve issue for short features when merging objects manually Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1133 diff changeset	1470 featureVelocities.addPosition(v)
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1471 # last position to feature
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1472 p = secondObject.getPositionAtInstant(emptyInterval.last)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1473 v = secondObject.getVelocityAtInstant(emptyInterval.last)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1474 featurePositions.addPosition(p)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1475 featureVelocities.addPosition(v)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1476 # copy second trajectory
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1477 positions.append(secondObject.getPositions())
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1478 velocities.append(secondObject.getVelocities())
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1479 newObject = MovingObject(newNum, TimeInterval(firstObject.getFirstInstant(), secondObject.getLastInstant()), positions, velocities, nObjects = 1)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1480 newFeature = MovingObject(newFeatureNum, emptyInterval, featurePositions, featureVelocities)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1481 if hasattr(obj1, 'featureNumbers') and hasattr(obj2, 'featureNumbers'):
1019 5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1482 newObject.featureNumbers = obj1.featureNumbers+obj2.featureNumbers+[newFeatureNum]
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1483 if obj1.hasFeatures() and obj2.hasFeatures():
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1484 newObject.features = obj1.getFeatures()+obj2.getFeatures()+[newFeature]
1019 5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1485 else: # time intervals overlap
623 ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1486 newTimeInterval = TimeInterval.union(obj1.getTimeInterval(), obj2.getTimeInterval())
1041 fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1487 newObject = MovingObject(newNum, newTimeInterval, nObjects = 1) # hypothesis is that it's the same object being reunited
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1488 newFeature = None
1019 5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1489 if hasattr(obj1, 'featureNumbers') and hasattr(obj2, 'featureNumbers'):
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1490 newObject.featureNumbers = obj1.featureNumbers+obj2.featureNumbers
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1491 if obj1.hasFeatures() and obj2.hasFeatures():
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1492 newObject.features = obj1.getFeatures()+obj2.getFeatures()
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1493 newObject.updatePositions()
623 ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1494 else:
1019 5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1495 print('Cannot update object positions without features')
1018 d7afc59f6966 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1016 diff changeset	1496 # user type
d7afc59f6966 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1016 diff changeset	1497 if obj1.getUserType() != obj2.getUserType():
d7afc59f6966 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1016 diff changeset	1498 print('The two moving objects have different user types: obj1 {} obj2 {}'.format(userTypeNames[obj1.getUserType()], userTypeNames[obj2.getUserType()]))
1019 5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1499 newObject.setUserType(obj1.getUserType())
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1500 return newObject, newFeature
622 dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	1501
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1502 def getObjectInTimeInterval(self, inter):
54 c354d41ef7cd corrected code for usertype in movingobject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 49 diff changeset	1503 '''Returns a new object extracted from self,
c354d41ef7cd corrected code for usertype in movingobject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 49 diff changeset	1504 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	1505 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	1506 if not intersection.empty():
b3a1c26e2f22 corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 96 diff changeset	1507 trajectoryInterval = TimeInterval(intersection.first-self.getFirstInstant(), intersection.last-self.getFirstInstant())
1146 b219d5a1bb55 added code to categorize interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1136 diff changeset	1508 obj = MovingObject(self.num, intersection, self.positions.subTrajectoryInInterval(trajectoryInterval), self.geometry, self.userType, self.nObjects)
982 51d8406b2489 corrected bug when not using a homography and using a mask for CLEAR MOT metrics Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 978 diff changeset	1509 if self.velocities is not None:
1146 b219d5a1bb55 added code to categorize interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1136 diff changeset	1510 obj.velocities = self.velocities.subTrajectoryInInterval(trajectoryInterval)
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1511 return obj
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1512 else:
978 184f1dd307f9 corrected print and exception statements for Python 3 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 959 diff changeset	1513 print('The object does not exist at {}'.format(inter))
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1514 return None
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1515
726 43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1516 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	1517 '''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	1518 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	1519 if homography is not None:
936 56cc8a1f7082 removed all old versions of projection methods Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 935 diff changeset	1520 self.projectedPositions = self.positions.homographyProject(homography)
726 43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1521 else:
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1522 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	1523 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	1524 p = positions[i]
982 51d8406b2489 corrected bug when not using a homography and using a mask for CLEAR MOT metrics Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 978 diff changeset	1525 return mask[int(p.y), int(p.x)] != 0.
726 43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1526
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1527 #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	1528 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	1529 # '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	1530 l = 0
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1531 intervalLabels = []
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1532 prev = True
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1533 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	1534 if i:
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1535 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	1536 l += 1
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1537 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	1538 else:
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1539 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	1540 prev = i
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1541 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	1542 subObjects = []
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1543 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	1544 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	1545 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	1546 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	1547 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	1548 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	1549
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1550 return subObjects
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1551
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1552 def getPositions(self):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1553 return self.positions
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1554
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1555 def getVelocities(self):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1556 return self.velocities
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1557
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	1558 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	1559 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	1560
943 b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 939 diff changeset	1561 def computeCumulativeDistances(self):
b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 939 diff changeset	1562 self.positions.computeCumulativeDistances()
b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 939 diff changeset	1563
335 3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1564 def getCurvilinearPositions(self):
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1565 if hasattr(self, 'curvilinearPositions'):
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1566 return self.curvilinearPositions
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1567 else:
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1568 return None
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1569
1123 0548a78852b8 added accessor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1122 diff changeset	1570 def getCurvilinearVelocities(self):
0548a78852b8 added accessor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1122 diff changeset	1571 if hasattr(self, 'curvilinearVelocities'):
0548a78852b8 added accessor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1122 diff changeset	1572 return self.curvilinearVelocities
0548a78852b8 added accessor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1122 diff changeset	1573 else:
0548a78852b8 added accessor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1122 diff changeset	1574 return None
0548a78852b8 added accessor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1122 diff changeset	1575
643 bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1576 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	1577 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	1578 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	1579 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	1580 if withOrigin:
994dd644f6ab corrected impact of warmUpLastInstant Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 659 diff changeset	1581 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	1582 else:
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1583 instants = []
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1584 coords = []
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1585 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	1586 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	1587 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	1588 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	1589 else:
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1590 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	1591 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	1592 plot(instants, coords, options, **kwargs)
660 994dd644f6ab corrected impact of warmUpLastInstant Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 659 diff changeset	1593 if withOrigin and len(instants)>0:
994dd644f6ab corrected impact of warmUpLastInstant Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 659 diff changeset	1594 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	1595 else:
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	1596 print('Object {} has no curvilinear positions'.format(self.getNum()))
643 bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1597
1116 a3982d591a61 placeholder implementation for interpolateCurvilinearPositions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1115 diff changeset	1598 def interpolateCurvilinearPositions(self, t, alignments = None):
1097 b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1599 '''Linear interpolation of curvilinear positions, t being a float'''
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1600 if hasattr(self, 'curvilinearPositions'):
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1601 if self.existsAtInstant(t):
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1602 i = int(floor(t))
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1603 p1 = self.getCurvilinearPositionAtInstant(i)
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1604 p2 = self.getCurvilinearPositionAtInstant(i+1)
1114 7135b5eaa6b4 correcting poor requirement for interpolateCurvilinearPositions (when changing alignment) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1112 diff changeset	1605 if p1[2] == p2[2]:
7135b5eaa6b4 correcting poor requirement for interpolateCurvilinearPositions (when changing alignment) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1112 diff changeset	1606 alpha = t-float(i)
7135b5eaa6b4 correcting poor requirement for interpolateCurvilinearPositions (when changing alignment) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1112 diff changeset	1607 return [(1-alpha)p1[0]+alphap2[0], (1-alpha)p1[1]+alphap2[1], p1[2]]
1117 bb58506c1bfd minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1116 diff changeset	1608 elif alignments is not None: # can be done only if assuming there is no missing alignmentn where the object has no coordinate
1116 a3982d591a61 placeholder implementation for interpolateCurvilinearPositions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1115 diff changeset	1609 pass # TODO
1097 b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1610 else:
1116 a3982d591a61 placeholder implementation for interpolateCurvilinearPositions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1115 diff changeset	1611 print('Object {} changes lane at {} and alignments are not provided'.format(self.getNum(), t))
1097 b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1612 else:
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1613 print('Object {} does not exist at {}'.format(self.getNum(), t))
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1614 else:
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1615 print('Object {} has no curvilinear positions'.format(self.getNum()))
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1616
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	1617 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	1618 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	1619
1041 fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1620 def getNObjects(self):
fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1621 return self.nObjects
fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1622
fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1623 def setNObjects(self, nObjects):
fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1624 if nObjects is None or nObjects >= 1:
fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1625 self.nObjects = nObjects
fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1626 else:
fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1627 print('Number of objects represented by object {} must be greater or equal to 1 ({})'.format(self.getNum(), nObjects))
1075 67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1628 self.nObjects = None
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1629
880 000555430b28 adapted code from Paul St-Aubin and udpated MovingObject.setFeatures to truly find the right features in a list that may not start at 0 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 877 diff changeset	1630 def setFeatures(self, features, featuresOrdered = False):
000555430b28 adapted code from Paul St-Aubin and udpated MovingObject.setFeatures to truly find the right features in a list that may not start at 0 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 877 diff changeset	1631 '''Sets the features in the features field based on featureNumbers
000555430b28 adapted code from Paul St-Aubin and udpated MovingObject.setFeatures to truly find the right features in a list that may not start at 0 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 877 diff changeset	1632 if not all features are loaded from 0, one needs to renumber in a dict'''
000555430b28 adapted code from Paul St-Aubin and udpated MovingObject.setFeatures to truly find the right features in a list that may not start at 0 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 877 diff changeset	1633 if featuresOrdered:
000555430b28 adapted code from Paul St-Aubin and udpated MovingObject.setFeatures to truly find the right features in a list that may not start at 0 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 877 diff changeset	1634 tmp = features
000555430b28 adapted code from Paul St-Aubin and udpated MovingObject.setFeatures to truly find the right features in a list that may not start at 0 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 877 diff changeset	1635 else:
000555430b28 adapted code from Paul St-Aubin and udpated MovingObject.setFeatures to truly find the right features in a list that may not start at 0 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 877 diff changeset	1636 tmp = {f.getNum():f for f in features}
000555430b28 adapted code from Paul St-Aubin and udpated MovingObject.setFeatures to truly find the right features in a list that may not start at 0 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 877 diff changeset	1637 self.features = [tmp[i] for i in self.featureNumbers]
268 0c0b92f621f6 reorganized to compute evasive action for multiple positions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 259 diff changeset	1638
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	1639 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	1640 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	1641
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	1642 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	1643 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	1644
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	1645 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	1646 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	1647 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	1648 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	1649 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	1650
869 eb2f8ce2b39d added method for longest feature Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 863 diff changeset	1651 def getNLongestFeatures(self, nFeatures = 1):
eb2f8ce2b39d added method for longest feature Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 863 diff changeset	1652 if self.features is None:
eb2f8ce2b39d added method for longest feature Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 863 diff changeset	1653 return []
eb2f8ce2b39d added method for longest feature Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 863 diff changeset	1654 else:
eb2f8ce2b39d added method for longest feature Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 863 diff changeset	1655 tmp = utils.sortByLength(self.getFeatures(), reverse = True)
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1656 return tmp[:min(len(tmp), nFeatures)]
9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1657
1044 75a6ad604cc5 work on motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1041 diff changeset	1658 def getFeatureNumbersOverTime(self):
666 93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1659 '''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	1660 dict instant -> number of features'''
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1661 if self.hasFeatures():
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1662 featureNumbers = {}
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1663 for t in self.getTimeInterval():
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1664 n = 0
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1665 for f in self.getFeatures():
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1666 if f.existsAtInstant(t):
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1667 n += 1
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1668 featureNumbers[t]=n
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1669 return featureNumbers
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1670 else:
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1671 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	1672 return None
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1673
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1674 def getSpeeds(self, nInstantsIgnoredAtEnds = 0):
1197 0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1675 '''Returns the scalar speed'''
682 fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1676 speeds = self.getVelocities().norm()
fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1677 if nInstantsIgnoredAtEnds > 0:
fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1678 n = min(nInstantsIgnoredAtEnds, int(floor(self.length()/2.)))
fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1679 return speeds[n:-n]
fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1680 else:
fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1681 return speeds
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	1682
1197 0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1683 def getAccelerations(self, fromSpeeds = True, nInstantsIgnoredAtEnds = 0):
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1684 '''Returns the scalar acceleration by differentiating the speeds (fromSpeeds = True) or by differentiating the velocity (vector) and taking the norm (frompSpeeds = False. In the second case, speed is always positive'''
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1685 if fromSpeeds:
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1686 speeds = array(self.getSpeeds())
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1687 accelerations = speeds[1:]-speeds[:-1]
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1688 else:
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1689 accelerations = self.getVelocities().differentiate().norm()
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1690 if nInstantsIgnoredAtEnds > 0:
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1691 n = min(nInstantsIgnoredAtEnds, int(floor(len(accelerations)/2.)))
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1692 return accelerations[n:-n]
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1693 else:
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1694 return accelerations
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1695
1196 d5566af60a69 correcting bug with savgol filter, renaming the functions to proper names filterSG and getAccelerationsSG Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1195 diff changeset	1696 def getAccelerationsSG(self, window_length, polyorder, delta=1.0, axis=-1, mode='nearest', cval=0.0, nInstantsIgnoredAtEnds = 0):
877 d1ff6917d082 added savitzky golay filter for accelerations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 869 diff changeset	1697 '''Returns the 1-D acceleration from the 1-D speeds
d1ff6917d082 added savitzky golay filter for accelerations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 869 diff changeset	1698 Caution about previously filtered data'''
1193 d324305c1240 solving issues with extreme values when filtering using savitsky golay Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1190 diff changeset	1699 speeds = self.getSpeeds()
1195 27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	1700 wlength = min(window_length, len(speeds))
27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	1701 if wlength % 2 == 0:
27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	1702 wlength -=1
1193 d324305c1240 solving issues with extreme values when filtering using savitsky golay Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1190 diff changeset	1703 filtered = savgol_filter(speeds, wlength, min(wlength-1, polyorder), 1, delta, axis, mode, cval)
d324305c1240 solving issues with extreme values when filtering using savitsky golay Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1190 diff changeset	1704 if nInstantsIgnoredAtEnds >= 1:
1195 27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	1705 if nInstantsIgnoredAtEnds >= len(speeds)/2:
27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	1706 n = int(round(len(speeds)/2))-1
27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	1707 else:
27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	1708 n = nInstantsIgnoredAtEnds
27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	1709 return filtered[n:-n]
1193 d324305c1240 solving issues with extreme values when filtering using savitsky golay Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1190 diff changeset	1710 else:
d324305c1240 solving issues with extreme values when filtering using savitsky golay Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1190 diff changeset	1711 return filtered
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1712
633 f410c8fb07b7 minor function to export speed as temporal indicator for display Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 631 diff changeset	1713 def getSpeedIndicator(self):
f410c8fb07b7 minor function to export speed as temporal indicator for display Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 631 diff changeset	1714 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	1715 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	1716
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1717 def getPositionAt(self, i):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1718 return self.positions[i]
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1719
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1720 def getVelocityAt(self, i):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1721 return self.velocities[i]
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1722
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1723 def getPositionAtInstant(self, i):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1724 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	1725
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1726 def getVelocityAtInstant(self, i):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1727 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	1728
1097 b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1729 def getCurvilinearPositionAt(self, i):
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1730 return self.curvilinearPositions[i]
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1731
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1732 def getCurvilinearVelocityAt(self, i):
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1733 return self.curvilinearVelocities[i]
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1734
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1735 def getCurvilinearPositionAtInstant(self, i):
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1736 return self.curvilinearPositions[i-self.getFirstInstant()]
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1737
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1738 def getCurvilinearVelocityAtInstant(self, i):
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1739 return self.curvilinearVelocities[i-self.getFirstInstant()]
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1740
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1741 def getXCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1742 return self.positions.getXCoordinates()
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1743
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1744 def getYCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1745 return self.positions.getYCoordinates()
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1746
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1747 def plot(self, options = '', withOrigin = False, timeStep = 1, withFeatures = False, withIds = False, **kwargs):
07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1748 if withIds:
07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1749 objNum = self.getNum()
07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1750 else:
07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1751 objNum = None
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	1752 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	1753 for f in self.getFeatures():
517 47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1754 f.positions.plot('r', True, timeStep, **kwargs)
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1755 self.positions.plot('bx-', True, timeStep, objNum, **kwargs)
517 47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1756 else:
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1757 self.positions.plot(options, withOrigin, timeStep, objNum, **kwargs)
182 d3f6de6c3918 added drawing functialities, in particular on aerial image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 152 diff changeset	1758
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1759 def plotOnWorldImage(self, nPixelsPerUnitDistance, options = '', withOrigin = False, timeStep = 1, withIds = False, **kwargs):
07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1760 if withIds:
07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1761 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, self.getNum(), **kwargs)
07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1762 else:
07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1763 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, None, **kwargs)
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	1764
1210 1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1765 def plotOutlineAtInstant(self, t, options = '', withVelocity = False, velocityMultiply = 5, arrowWidth=0.1, **kwargs):
1203 7b3384a8e409 second version of code loading kitti data, to clean Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1197 diff changeset	1766 if self.hasFeatures():
7b3384a8e409 second version of code loading kitti data, to clean Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1197 diff changeset	1767 points = [f.getPositionAtInstant(t) for f in self.getFeatures()]
1205 3905b393ade0 kitti loading code seems to be working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1203 diff changeset	1768 Point.plotAll(points, True, options, **kwargs)
1210 1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1769 if withVelocity:
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1770 p = self.getPositionAtInstant(t)
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1771 v = self.getVelocityAtInstant(t)*velocityMultiply
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1772 arrow(p.x, p.y, v.x, v.y, width=arrowWidth)
1203 7b3384a8e409 second version of code loading kitti data, to clean Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1197 diff changeset	1773
626 35155ac2a294 corrected bugs, in particular to MovingObject.play Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 624 diff changeset	1774 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	1775 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	1776
877 d1ff6917d082 added savitzky golay filter for accelerations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 869 diff changeset	1777 def speedDiagnostics(self, framerate = 1., display = False, nInstantsIgnoredAtEnds=0):
d1ff6917d082 added savitzky golay filter for accelerations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 869 diff changeset	1778 speeds = framerate*self.getSpeeds(nInstantsIgnoredAtEnds)
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	1779 coef = utils.linearRegression(list(range(len(speeds))), speeds)
379 f1a1923ddff9 minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 378 diff changeset	1780 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	1781 if display:
672 5473b7460375 moved and rationalized imports in modules Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 666 diff changeset	1782 from matplotlib.pyplot import figure, axis
377 2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1783 figure(1)
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	1784 self.plot()
379 f1a1923ddff9 minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 378 diff changeset	1785 axis('equal')
377 2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1786 figure(2)
2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1787 plot(list(self.getTimeInterval()), speeds)
877 d1ff6917d082 added savitzky golay filter for accelerations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 869 diff changeset	1788 figure(3)
d1ff6917d082 added savitzky golay filter for accelerations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 869 diff changeset	1789 plot(list(self.getTimeInterval()), self.getAccelerations(9, 3, speeds = speeds)) # arbitrary parameter
377 2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1790
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1791 @staticmethod
653 107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1792 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	1793 '''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	1794 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	1795 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	1796 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	1797 maxDistance = minDistance
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1798 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	1799 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	1800 if d<minDistance:
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1801 minDistance = d
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1802 elif d>maxDistance:
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1803 maxDistance = d
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1804 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	1805 else:
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1806 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	1807
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1808 @staticmethod
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1809 def distances(obj1, obj2, instant1, _instant2 = None):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1810 '''Returns the distances between all features of the 2 objects
653 107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1811 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	1812 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	1813 if _instant2 is None:
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1814 instant2 = instant1
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1815 else:
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1816 instant2 = _instant2
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1817 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	1818 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	1819 return cdist(positions1, positions2, metric = 'euclidean')
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1820
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1821 @staticmethod
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1822 def minDistance(obj1, obj2, instant1, instant2 = None):
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1823 return MovingObject.distances(obj1, obj2, instant1, instant2).min()
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1824
6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1825 @staticmethod
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1826 def maxDistance(obj1, obj2, instant, instant2 = None):
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1827 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	1828
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1829 def maxSize(self):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1830 '''Returns the max distance between features
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1831 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	1832 if hasattr(self, 'features'):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1833 nFeatures = -1
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1834 tMaxFeatures = 0
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1835 for t in self.getTimeInterval():
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1836 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	1837 if n > nFeatures:
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1838 nFeatures = n
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1839 tMaxFeatures = t
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1840 return MovingObject.maxDistance(self, self, tMaxFeatures)
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1841 else:
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1842 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	1843 return None
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1844
550 5668af2ff515 minor naming Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 549 diff changeset	1845 def setRoutes(self, startRouteID, endRouteID):
5668af2ff515 minor naming Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 549 diff changeset	1846 self.startRouteID = startRouteID
5668af2ff515 minor naming Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 549 diff changeset	1847 self.endRouteID = endRouteID
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1848
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1849 def getInstantsCrossingLine(self, p1, p2, computeOrientations = False):
55 88d5ee5ac164 updated comments and added shell for interaction between road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 54 diff changeset	1850 '''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	1851 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	1852 empty list if there is no crossing'''
1170 b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1853 indices, intersections, rightToLeftOrientations = self.positions.getIntersections(p1, p2, computeOrientations)
b55adb13f262 added functions on line crossing orientation and important reorganization and cleaning of related code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1146 diff changeset	1854 return [t+self.getFirstInstant() for t in indices], intersections, rightToLeftOrientations
27 44689029a86f updated segmentIntersection and other Nicolas Saunier <nico@confins.net> parents: 26 diff changeset	1855
949 d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	1856 def computeTrajectorySimilarities(self, prototypes, lcss):
d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	1857 'Computes the similarities to the prototypes using the LCSS'
d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	1858 if not hasattr(self, 'prototypeSimilarities'):
d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	1859 self.prototypeSimilarities = []
d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	1860 for proto in prototypes:
d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	1861 lcss.similarities(proto.getMovingObject().getPositions().asArray().T, self.getPositions().asArray().T)
d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	1862 similarities = lcss.similarityTable[-1, :-1].astype(float)
1109 6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1863 self.prototypeSimilarities.append(similarities/minimum(arange(1., len(similarities)+1), proto.getMovingObject().length()*ones(len(similarities))))
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1864
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	1865 @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	1866 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	1867 '''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	1868
887 e2452abba0e7 added option to compute PET in safety analysis script, and save in database Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 881 diff changeset	1869 Returns the smallest time difference when the object positions are within collisionDistanceThreshold
e2452abba0e7 added option to compute PET in safety analysis script, and save in database Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 881 diff changeset	1870 and the instants at which each object is passing through its corresponding position'''
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	1871 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	1872 positions2 = [p.astuple() for p in obj2.getPositions()]
887 e2452abba0e7 added option to compute PET in safety analysis script, and save in database Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 881 diff changeset	1873 n1 = len(positions1)
e2452abba0e7 added option to compute PET in safety analysis script, and save in database Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 881 diff changeset	1874 n2 = len(positions2)
e2452abba0e7 added option to compute PET in safety analysis script, and save in database Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 881 diff changeset	1875 pets = zeros((n1, n2))
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	1876 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	1877 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	1878 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	1879 distances = cdist(positions1, positions2, metric = 'euclidean')
887 e2452abba0e7 added option to compute PET in safety analysis script, and save in database Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 881 diff changeset	1880 smallDistances = (distances <= collisionDistanceThreshold)
e2452abba0e7 added option to compute PET in safety analysis script, and save in database Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 881 diff changeset	1881 if smallDistances.any():
e2452abba0e7 added option to compute PET in safety analysis script, and save in database Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 881 diff changeset	1882 smallPets = pets[smallDistances]
e2452abba0e7 added option to compute PET in safety analysis script, and save in database Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 881 diff changeset	1883 petIdx = smallPets.argmin()
e2452abba0e7 added option to compute PET in safety analysis script, and save in database Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 881 diff changeset	1884 distanceIndices = argwhere(smallDistances)[petIdx]
e2452abba0e7 added option to compute PET in safety analysis script, and save in database Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 881 diff changeset	1885 return smallPets[petIdx], obj1.getFirstInstant()+distanceIndices[0], obj2.getFirstInstant()+distanceIndices[1]
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	1886 else:
887 e2452abba0e7 added option to compute PET in safety analysis script, and save in database Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 881 diff changeset	1887 return None, None, None
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	1888
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	1889 def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1890 '''Predicts the position of object at instant+deltaT,
243 e0988a8ace0c started adapting and moving to other modules Mohamed's work Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 223 diff changeset	1891 at constant speed'''
255 13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	1892 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	1893
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1894 def projectCurvilinear(self, alignments, halfWidth = 3):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1895 self.curvilinearPositions = CurvilinearTrajectory.fromTrajectoryProjection(self.getPositions(), alignments, halfWidth)
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1896
562 259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1897 def computeSmoothTrajectory(self, minCommonIntervalLength):
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1898 '''Computes the trajectory as the mean of all features
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1899 if a feature exists, its position is
9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1900
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1901 Warning work in progress
562 259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1902 TODO? not use the first/last 1-.. positions'''
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1903 nFeatures = len(self.features)
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1904 if nFeatures == 0:
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1905 print('Empty object features\nCannot compute smooth trajectory')
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1906 else:
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1907 # compute the relative position vectors
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1908 relativePositions = {} # relativePositions[(i,j)] is the position of j relative to i
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	1909 for i in range(nFeatures):
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	1910 for j in range(i):
562 259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1911 fi = self.features[i]
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1912 fj = self.features[j]
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1913 inter = fi.commonTimeInterval(fj)
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1914 if inter.length() >= minCommonIntervalLength:
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1915 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	1916 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	1917 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	1918 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	1919 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	1920 relativePositions[(j,i)] = -relativePositions[(i,j)]
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1921
1212 af329f3330ba work in progress on 3D safety analysis Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1210 diff changeset	1922 def getBoundingPolygon(self, t, shape = 'raw'):
1106 799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	1923 '''Returns a bounding box for the feature positions at instant
1210 1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1924 bounding box format is a list of points
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1925 shape is the type of output shape:
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1926 - raw means the list of feature positions
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1927 - rect means the bounding rectangle aligned with velocity'''
1106 799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	1928 if self.hasFeatures():
1210 1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1929 positions = [f.getPositionAtInstant(t) for f in self.getFeatures() if f.existsAtInstant(t)]
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1930 if shape == 'rect':
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1931 return Point.boundingRectangle(positions, self.getVelocityAtInstant(t))
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1932 elif shape == 'raw':
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1933 return positions
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1934 else:
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1935 print('Unknown shape')
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1936 return None
1106 799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	1937 else:
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	1938 print('Object {} has no features'.format(self.getNum()))
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	1939 return None
1109 6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1940
1132 09ef0dc994a0 modification for performance computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1126 diff changeset	1941 def motDistanceAtInstant(self, obj, instant):
09ef0dc994a0 modification for performance computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1126 diff changeset	1942 '''Returns distance for computing CLEAR MOT metrics
09ef0dc994a0 modification for performance computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1126 diff changeset	1943 (returns an actual value, otherwise munkres does not terminate)'''
09ef0dc994a0 modification for performance computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1126 diff changeset	1944 return Point.distanceNorm2(self.getPositionAtInstant(instant), obj.getPositionAtInstant(instant))
09ef0dc994a0 modification for performance computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1126 diff changeset	1945
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	1946 ###
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1947 # 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	1948 ###
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1949 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	1950 '''Classifies slow and fast road users
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1951 slow: non-motorized -> pedestrians
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 583 diff changeset	1952 fast: motorized -> cars
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1953
608 078adacd72a4 moving.py integrating Mohamed's comments refactored Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 607 diff changeset	1954 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	1955 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	1956 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	1957 if aggregationFunc(speeds) >= threshold:
345 fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1958 self.setUserType(userType2Num['car'])
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1959 else:
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1960 self.setUserType(userType2Num['pedestrian'])
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1961
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1962 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	1963 '''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	1964 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	1965 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	1966 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	1967
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	1968 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	1969 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	1970 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	1971 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	1972 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	1973 return x'''
626 35155ac2a294 corrected bugs, in particular to MovingObject.play Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 624 diff changeset	1974 if not hasattr(self, 'aggregatedSpeed'):
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1975 self.aggregatedSpeed = aggregationFunc(self.getSpeeds(nInstantsIgnoredAtEnds))
581 10e8a9f2bd9f change for python 2.6 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 579 diff changeset	1976 userTypeProbabilities = {}
10e8a9f2bd9f change for python 2.6 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 579 diff changeset	1977 for userTypename in speedProbabilities:
10e8a9f2bd9f change for python 2.6 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 579 diff changeset	1978 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	1979 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	1980 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	1981
928 063d1267585d work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 921 diff changeset	1982 def initClassifyUserTypeHoGSVM(self, aggregationFunc, pedBikeCarSVM, bikeCarSVM = None, pedBikeSpeedTreshold = float('Inf'), bikeCarSpeedThreshold = float('Inf'), nInstantsIgnoredAtEnds = 0, homography = None, intrinsicCameraMatrix = None, distortionCoefficients = 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	1983 '''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	1984
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1985 TODO? compute speed for longest feature?'''
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1986 self.aggregatedSpeed = aggregationFunc(self.getSpeeds(nInstantsIgnoredAtEnds))
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1987 if self.aggregatedSpeed < pedBikeSpeedTreshold or bikeCarSVM is None:
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1988 self.appearanceClassifier = pedBikeCarSVM
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1989 elif self.aggregatedSpeed < bikeCarSpeedThreshold:
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1990 self.appearanceClassifier = bikeCarSVM
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1991 else:
959 4f32d82ca390 corrected error due to change in Hog (scikit image) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 956 diff changeset	1992 self.appearanceClassifier = carClassifier
928 063d1267585d work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 921 diff changeset	1993 # project feature positions
063d1267585d work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 921 diff changeset	1994 if self.hasFeatures():
063d1267585d work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 921 diff changeset	1995 for f in self.getFeatures():
935 0e63a918a1ca updated classify-objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 933 diff changeset	1996 pp = cvutils.worldToImageProject(f.getPositions().asArray(), intrinsicCameraMatrix, distortionCoefficients, homography).tolist()
929 be28a3538dc9 work in progress on projection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 928 diff changeset	1997 f.positions = Trajectory(pp)
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	1998 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	1999
929 be28a3538dc9 work in progress on projection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 928 diff changeset	2000 def classifyUserTypeHoGSVMAtInstant(self, img, instant, width, height, px, py, minNPixels, rescaleSize, orientations, pixelsPerCell, cellsPerBlock, blockNorm):
812 21f10332c72b moved the classification parameters from tracking.cfg to a new classifier.cfg and made all classification parameters apparent Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 785 diff changeset	2001 '''Extracts the image box around the object
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	2002 (of square size max(width, height) of the box around the features,
812 21f10332c72b moved the classification parameters from tracking.cfg to a new classifier.cfg and made all classification parameters apparent Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 785 diff changeset	2003 with an added px or py for width and height (around the box))
21f10332c72b moved the classification parameters from tracking.cfg to a new classifier.cfg and made all classification parameters apparent Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 785 diff changeset	2004 computes HOG on this cropped image (with parameters rescaleSize, orientations, pixelsPerCell, cellsPerBlock)
21f10332c72b moved the classification parameters from tracking.cfg to a new classifier.cfg and made all classification parameters apparent Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 785 diff changeset	2005 and applies the SVM model on it'''
929 be28a3538dc9 work in progress on projection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 928 diff changeset	2006 croppedImg = cvutils.imageBox(img, self, instant, width, height, px, py, minNPixels)
685 94b291a5f933 several updates for display Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 682 diff changeset	2007 if croppedImg is not None and len(croppedImg) > 0:
959 4f32d82ca390 corrected error due to change in Hog (scikit image) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 956 diff changeset	2008 hog = cvutils.HOG(croppedImg, rescaleSize, orientations, pixelsPerCell, cellsPerBlock, blockNorm)
993 e8eabef7857c update to OpenCV3 for python Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 987 diff changeset	2009 self.userTypes[instant] = self.appearanceClassifier.predict(hog.reshape(1,hog.size))
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	2010 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	2011 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	2012
898 1fc901d983ed better take into account unknown appearance classification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 893 diff changeset	2013 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, maxPercentUnknown = 0.5, nInstantsIgnoredAtEnds = 0, px = 0.2, py = 0.2, minNPixels = 800, rescaleSize = (64, 64), orientations = 9, pixelsPerCell = (8,8), cellsPerBlock = (2,2)):
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	2014 '''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	2015 (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	2016
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	2017 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	2018 With default parameters, the general (ped-bike-car) classifier will be used
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	2019
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	2020 Considered categories are the keys of speedProbabilities'''
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	2021 if not hasattr(self, 'aggregatedSpeed') or not hasattr(self, 'userTypes'):
845 aa98e773ac91 minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 828 diff changeset	2022 print('Initializing the data structures for classification by HoG-SVM')
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	2023 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	2024
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	2025 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	2026 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	2027 if t not in self.userTypes:
812 21f10332c72b moved the classification parameters from tracking.cfg to a new classifier.cfg and made all classification parameters apparent Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 785 diff changeset	2028 self.classifyUserTypeHoGSVMAtInstant(images[t], t, homography, width, height, px, py, minNPixels, rescaleSize, orientations, pixelsPerCell, cellsPerBlock)
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	2029 # compute P(Speed\|Class)
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	2030 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	2031 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	2032 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	2033 userTypeProbabilities = {userType2Num[userTypename]: speedProbabilities[userTypename](self.aggregatedSpeed) for userTypename in speedProbabilities}
898 1fc901d983ed better take into account unknown appearance classification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 893 diff changeset	2034 # compute P(Class\|Appearance)
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	2035 nInstantsUserType = {userTypeNum: 0 for userTypeNum in userTypeProbabilities}# number of instants the object is classified as userTypename
898 1fc901d983ed better take into account unknown appearance classification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 893 diff changeset	2036 nInstantsUserType[userType2Num['unknown']] = 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	2037 for t in self.userTypes:
898 1fc901d983ed better take into account unknown appearance classification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 893 diff changeset	2038 nInstantsUserType[self.userTypes[t]] += 1 #nInstantsUserType.get(self.userTypes[t], 0) + 1
1fc901d983ed better take into account unknown appearance classification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 893 diff changeset	2039 # result is P(Class\|Appearance) x P(Speed\|Class)
1fc901d983ed better take into account unknown appearance classification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 893 diff changeset	2040 if nInstantsUserType[userType2Num['unknown']] < maxPercentUnknown*self.length(): # if not too many unknowns
1fc901d983ed better take into account unknown appearance classification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 893 diff changeset	2041 for userTypeNum in userTypeProbabilities:
1fc901d983ed better take into account unknown appearance classification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 893 diff changeset	2042 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	2043 # class is the user type that maximizes usertype probabilities
898 1fc901d983ed better take into account unknown appearance classification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 893 diff changeset	2044 if nInstantsUserType[userType2Num['unknown']] >= maxPercentUnknown*self.length() and (speedProbabilities is None or self.aggregatedSpeed < minSpeedEquiprobable): # if no speed information and too many unknowns
1fc901d983ed better take into account unknown appearance classification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 893 diff changeset	2045 self.setUserType(userType2Num['unknown'])
1fc901d983ed better take into account unknown appearance classification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 893 diff changeset	2046 else:
1fc901d983ed better take into account unknown appearance classification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 893 diff changeset	2047 self.setUserType(utils.argmaxDict(userTypeProbabilities))
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	2048
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	2049 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	2050 '''Classifies the object based on its location (projected to image space)
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	2051 areas is a dictionary of matrix of the size of the image space
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	2052 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	2053
ce40a89bd6ae added functions for classification refactored from Sohail s work for TRB/TRC (to be tested) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 520 diff changeset	2054 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	2055 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	2056 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	2057 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	2058 if homography is not None:
936 56cc8a1f7082 removed all old versions of projection methods Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 935 diff changeset	2059 self.projectedPositions = obj.positions.homographyProject(homography)
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	2060 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	2061 self.projectedPositions = obj.positions
523 ce4eaabacc26 modified internal implementation of user type for classifyUserTypeArea Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 522 diff changeset	2062 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	2063 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	2064 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	2065 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	2066 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	2067 # 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	2068 return possibleUserTypes
520 fd9641cbd24b added function to classify object at instant from SVM Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 517 diff changeset	2069
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	2070 @staticmethod
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	2071 def collisionCourseDotProduct(movingObject1, movingObject2, instant):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	2072 '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	2073 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	2074 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	2075 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	2076
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	2077 @staticmethod
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	2078 def collisionCourseCosine(movingObject1, movingObject2, instant):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	2079 '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	2080 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	2081 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	2082
587 cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	2083
921 630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2084 class Prototype(object):
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2085 'Class for a prototype'
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2086
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2087 def __init__(self, filename, num, trajectoryType, nMatchings = None):
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2088 self.filename = filename
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2089 self.num = num
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2090 self.trajectoryType = trajectoryType
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2091 self.nMatchings = nMatchings
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2092 self.movingObject = None
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2093
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2094 def getFilename(self):
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2095 return self.filename
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2096 def getNum(self):
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2097 return self.num
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2098 def getTrajectoryType(self):
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2099 return self.trajectoryType
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2100 def getNMatchings(self):
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2101 return self.nMatchings
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2102 def getMovingObject(self):
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2103 return self.movingObject
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2104 def setMovingObject(self, o):
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2105 self.movingObject = o
1035 933588568bec major update to learn motion pattern, see program description Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1030 diff changeset	2106 def __str__(self):
933588568bec major update to learn motion pattern, see program description Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1030 diff changeset	2107 return '{} {} {}'.format(self.filename, self.num, self.trajectoryType)
1037 6a6c37eb3a74 added function to load prototype assignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1035 diff changeset	2108 def __eq__(self, p2):
6a6c37eb3a74 added function to load prototype assignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1035 diff changeset	2109 return self.filename == p2.filename and self.num == p2.num and self.trajectoryType == p2.trajectoryType
6a6c37eb3a74 added function to load prototype assignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1035 diff changeset	2110 def __hash__(self):
6a6c37eb3a74 added function to load prototype assignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1035 diff changeset	2111 return hash((self.filename, self.num, self.trajectoryType))
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	2112
587 cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	2113 ##################
588 c5406edbcf12 added loading ground truth annotations (ground truth) from polytrack format Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 587 diff changeset	2114 # Annotations
587 cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	2115 ##################
cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	2116
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	2117 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	2118 '''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	2119 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	2120 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	2121
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	2122 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	2123
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	2124 Its center is the center of the box (generalize to other shapes?)
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	2125 (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	2126 '''
587 cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	2127
1132 09ef0dc994a0 modification for performance computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1126 diff changeset	2128 def __init__(self, topLeftPositions, bottomRightPositions, num = None, timeInterval = 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	2129 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	2130 self.topLeftPositions = topLeftPositions.getPositions()
5800a87f11ae corrected one bug and changed attribute names Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 588 diff changeset	2131 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	2132
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	2133 def computeCentroidTrajectory(self, homography = None):
939 a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	2134 self.positions = self.topLeftPositions.add(self.bottomRightPositions).__mul__(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	2135 if homography is not None:
936 56cc8a1f7082 removed all old versions of projection methods Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 935 diff changeset	2136 self.positions = self.positions.homographyProject(homography)
590 0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	2137
1081 346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2138 def matchObjects(obj1, obj2, instant, matchingDistance):
346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2139 '''Indicates if obj matches obj2 with threshold matchingDistance
346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2140 Returns distance if below matchingDistance, matchingDistance+1 otherwise
346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2141 (returns an actual value, otherwise munkres does not terminate)'''
346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2142 d = Point.distanceNorm2(obj1.getPositionAtInstant(instant), obj2.getPositionAtInstant(instant))
346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2143 if d < matchingDistance:
346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2144 return d
346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2145 else:
346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2146 return matchingDistance + 1
346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2147
346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2148 # TODO class to have different matching methods, eg with something like matchObjects
723 e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	2149 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	2150 '''Computes the CLEAR MOT metrics
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2151
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2152 Reference:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2153 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	2154
1190 d24d57e4de24 work on optimization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1185 diff changeset	2155 objects and annotations are supposed to be 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	2156 current implementation is BBMovingObject (bounding boxes)
592 985a3021cff2 first match table implementation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 590 diff changeset	2157 mathingDistance is threshold on matching between annotation and object
985a3021cff2 first match table implementation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 590 diff changeset	2158
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2159 TO: tracker output (objects)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2160 GT: ground truth (annotations)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2161
624 bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	2162 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	2163 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	2164 mme number of mismatches
bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	2165 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	2166 gt number of GT.frames
bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	2167
723 e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	2168 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	2169 matches is a dict
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	2170 matches[i] is the list of matches for GT/TO i
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	2171 the list of matches is a dict, indexed by time, for the TO/GT id matched at time t
723 e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	2172 (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	2173
624 bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	2174 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	2175 (add argument useDistanceForWeights = False)'''
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2176 from munkres import Munkres
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	2177
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2178 munk = Munkres()
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2179 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	2180 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	2181 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	2182 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	2183 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	2184 mme = 0 # number of mismatches
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2185 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	2186 if returnMatches:
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	2187 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	2188 toMatches = {o.getNum():{} for o in objects}
956 196a1fd498ba removing unnecessary complexity of varying number of return elements Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 949 diff changeset	2189 else:
196a1fd498ba removing unnecessary complexity of varying number of return elements Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 949 diff changeset	2190 gtMatches = None
196a1fd498ba removing unnecessary complexity of varying number of return elements Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 949 diff changeset	2191 toMatches = None
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	2192 for t in range(firstInstant, lastInstant+1):
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2193 previousMatches = matches.copy()
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2194 # 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	2195 toDelete = []
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2196 for a in matches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2197 if a.existsAtInstant(t) and matches[a].existsAtInstant(t):
1132 09ef0dc994a0 modification for performance computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1126 diff changeset	2198 d = a.motDistanceAtInstant(matches[a], t)
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2199 if d < matchingDistance:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2200 dist += d
592 985a3021cff2 first match table implementation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 590 diff changeset	2201 else:
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2202 toDelete.append(a)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2203 else:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2204 toDelete.append(a)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2205 for a in toDelete:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2206 del matches[a]
593 e2a873e08568 non-working clear mot metrics Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 592 diff changeset	2207
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2208 # match all unmatched GT-TO
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	2209 matchedGTs = list(matches.keys())
933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	2210 matchedTOs = list(matches.values())
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2211 costs = []
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2212 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	2213 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	2214 nGTs = len(matchedGTs)+len(unmatchedGTs)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2215 nTOs = len(matchedTOs)+len(unmatchedTOs)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2216 if len(unmatchedTOs) > 0:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2217 for a in unmatchedGTs:
1133 c4d9c270f999 modification for performance computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1132 diff changeset	2218 costs.append([a.motDistanceAtInstant(o, t) for o in unmatchedTOs])
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2219 if len(costs) > 0:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2220 newMatches = munk.compute(costs)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2221 for k,v in newMatches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2222 if costs[k][v] < matchingDistance:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2223 matches[unmatchedGTs[k]]=unmatchedTOs[v]
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2224 dist += costs[k][v]
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2225 if debug:
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	2226 print('{} '.format(t)+', '.join(['{} {}'.format(k.getNum(), v.getNum()) for k,v in matches.items()]))
723 e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	2227 if returnMatches:
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	2228 for a,o in matches.items():
723 e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	2229 gtMatches[a.getNum()][t] = o.getNum()
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	2230 toMatches[o.getNum()][t] = a.getNum()
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	2231
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2232 # compute metrics elements
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2233 ct += len(matches)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2234 mt += nGTs-len(matches)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2235 fpt += nTOs-len(matches)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2236 gt += nGTs
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2237 # compute mismatches
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2238 # 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	2239 mismatches = []
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2240 for a in matches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2241 if a in previousMatches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2242 if matches[a] != previousMatches[a]:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2243 mismatches.append(a)
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	2244 elif matches[a] in list(previousMatches.values()):
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2245 mismatches.append(matches[a])
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2246 for a in previousMatches:
998 933670761a57 updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 997 diff changeset	2247 if a not in matches and previousMatches[a] in list(matches.values()):
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2248 mismatches.append(previousMatches[a])
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	2249 if debug:
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2250 for mm in set(mismatches):
978 184f1dd307f9 corrected print and exception statements for Python 3 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 959 diff changeset	2251 print('{} {}'.format(type(mm), mm.getNum()))
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2252 # 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	2253 mme += len(set(mismatches))
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	2254
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2255 if ct > 0:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2256 motp = dist/ct
593 e2a873e08568 non-working clear mot metrics Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 592 diff changeset	2257 else:
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2258 motp = None
595 17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	2259 if gt > 0:
17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	2260 mota = 1.-float(mt+fpt+mme)/gt
17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	2261 else:
17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	2262 mota = None
956 196a1fd498ba removing unnecessary complexity of varying number of return elements Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 949 diff changeset	2263 return motp, mota, mt, mme, fpt, gt, gtMatches, toMatches
593 e2a873e08568 non-working clear mot metrics Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 592 diff changeset	2264
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	2265 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	2266 '''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	2267 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	2268 figure()
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	2269 for obj in objects:
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	2270 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	2271 axis('equal')
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	2272
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	2273
1016 9b53be469a36 getting ready for pull request Wendlasida parents: 1014 diff changeset	2274 if __name__ == "__main__":
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	2275 import doctest
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	2276 import unittest
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	2277 suite = doctest.DocFileSuite('tests/moving.txt')
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	2278 #suite = doctest.DocTestSuite()
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	2279 unittest.TextTestRunner().run(suite)
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	2280 #doctest.testmod()
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	2281 #doctest.testfile("example.txt")
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	2282 if shapelyAvailable:
635 6ae68383071e corrected issue with tests requiring shapely, adding a separate test file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 633 diff changeset	2283 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	2284 unittest.TextTestRunner().run(suite)

Mercurial Hosting > traffic-intelligence

annotate trafficintelligence/moving.py @ 1213:3f2214125164