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

annotate trafficintelligence/moving.py @ 1215:1b472cddf9b1

updated reading by interpolating missing frames

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Fri, 05 May 2023 22:52:25 -0400
parents	01c24c1cdb70
children	5038c357b57f

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
1214 01c24c1cdb70 implemented direct method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1213 diff changeset	401 def timeToCollisionPoly(corners1, v1, corners2, v2):
1213 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
1214 01c24c1cdb70 implemented direct method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1213 diff changeset	408 from sympy.abc import t
1213 3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	409 def NewFourPoints(bbox, col_time, v_x, v_y):
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	410 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	411
1214 01c24c1cdb70 implemented direct method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1213 diff changeset	412 v_1_x, v_1_y = v1.x, v1.y
01c24c1cdb70 implemented direct method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1213 diff changeset	413 v_2_x, v_2_y = v2.x, v2.y
01c24c1cdb70 implemented direct method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1213 diff changeset	414 x_bbox_1 = [p.x for p in corners1]
01c24c1cdb70 implemented direct method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1213 diff changeset	415 y_bbox_1 = [p.y for p in corners1]
1213 3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	416 bbox_1 = [x_bbox_1, y_bbox_1]
1214 01c24c1cdb70 implemented direct method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1213 diff changeset	417 x_bbox_2 = [p.x for p in corners2]
01c24c1cdb70 implemented direct method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1213 diff changeset	418 y_bbox_2 = [p.y for p in corners2]
1213 3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	419 bbox_2 = [x_bbox_2, y_bbox_2]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	420 t_total = []
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	421 line = [[0, 1], [1, 2], [2, 3], [3, 0]]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	422 for i in range(4):
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	423 for j in range(4):
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	424 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	425 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	426 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	427 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	428 collision_t = solve(f, t, set=True, dict=True)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	429 if collision_t and collision_t[0][t] >= 0:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	430 collision_t = collision_t[0][t]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	431 print(f'collision_t: {collision_t} >>>>>>>>>')
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	432 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	433 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	434 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	435 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	436 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	437 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	438 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	439 if min_x == max_x:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	440 if min_y <= p0_y <= max_y:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	441 t_total.append(collision_t)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	442
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	443 elif min_y == max_y:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	444 if min_x <= p0_x <= max_x:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	445 t_total.append(collision_t)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	446
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	447 else:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	448 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	449 t_total.append(collision_t)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	450 x_temps = x_bbox_2
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	451 x_bbox_2 = x_bbox_1
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	452 x_bbox_1 = x_temps
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	453 # print(f'Change bounding box1 and box2 >>>>>>>>>>>>>>>>>>>\n\n\n')
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	454 for i in range(4):
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	455 for j in range(4):
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	456 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	457 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	458 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	459 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	460 collision_t = solve(f, t, set=True, dict=True)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	461 if collision_t and collision_t[0][t] >= 0:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	462 collision_t = collision_t[0][t]
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	463 # print(f'collision_t: {collision_t} >>>>>>>>>')
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	464 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	465 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	466 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	467 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	468 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	469 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	470 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	471 if min_x == max_x:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	472 if min_y <= p0_y <= max_y:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	473 t_total.append(collision_t)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	474
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	475 elif min_y == max_y:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	476 if min_x <= p0_x <= max_x:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	477 t_total.append(collision_t)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	478
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	479 else:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	480 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	481 t_total.append(collision_t)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	482
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	483 if t_total:
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	484 collision_t_min = min(t_total)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	485 # print(f'collision_time:')
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	486 # print(collision_t_min)
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	487 else:
1214 01c24c1cdb70 implemented direct method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1213 diff changeset	488 collision_t_min = None
1213 3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	489 # print(f'No collision')
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	490 return collision_t_min
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	491
3f2214125164 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1212 diff changeset	492 @staticmethod
583 6ebfb43e938e added midpoint function (from laurent gauthier) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 582 diff changeset	493 def midPoint(p1, p2):
6ebfb43e938e added midpoint function (from laurent gauthier) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 582 diff changeset	494 '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	495 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	496
1019 5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	497 @staticmethod
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	498 def agg(points, aggFunc = mean):
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	499 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	500
1106 799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	501 @staticmethod
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	502 def boundingRectangle(points, v):
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	503 '''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	504 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	505 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	506 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	507 xCoords = []
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	508 yCoords = []
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	509 for p in points:
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	510 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	511 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	512 xmin = min(xCoords)
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	513 xmax = max(xCoords)
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	514 ymin = min(yCoords)
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	515 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	516 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	517 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	518 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	519 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	520 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	521 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	522 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	523
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	524 if shapelyAvailable:
1210 1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	525 def pointsToShapely(points):
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	526 'Returns shapely polygon'
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	527 return Polygon([p.astuple() for p in points])
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	528
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	529 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	530 '''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	531 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	532 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	533 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	534 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	535 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	536
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	537 # Functions for coordinate transformation
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	538 # 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	539 def prepareAlignments(alignments):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	540 '''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	541 - computes cumulative distances
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	542 - 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	543 for alignment in alignments:
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	544 alignment.computeCumulativeDistances()
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	545 p1 = alignment[0]
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	546 for i in range(len(alignment)-1):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	547 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	548 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	549 p2.x += 0.0000000001
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	550 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	551 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	552 p1 = p2
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	553
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	554 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	555 ''' 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	556 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	557 '''
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	558 if(p0x == p1x and p0y == p1y):
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	559 return None
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	560 try:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	561 #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	562 # 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	563 # p1x += 0.0000000001
1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	564 # 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	565 # p1y += 0.0000000001
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	566 #make the calculation
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	567 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	568 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	569 except ZeroDivisionError:
072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	570 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	571 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	572 import pdb;
6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	573 pdb.set_trace()
6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	574 return Point(X, Y)
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	575
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	576 def getSYfromXY(p, alignments, goodEnoughAlignmentDistance = 0.5):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	577 ''' 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	578 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	579
659 784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	580 Output:
784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	581 =======
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	582 [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	583 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	584 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	585 subsegment distance,
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	586 alignment distance,
659 784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	587 orthogonal point offset]
784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	588
784298512b60 minor modifications Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 653 diff changeset	589 or None
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	590 '''
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	591 minOffsetY = float('inf')
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	592 #For each alignment
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	593 for alignmentIdx in range(len(alignments)):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	594 #For each alignment point index
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	595 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	596 #Get closest point on alignment
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	597 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	598 if closestPoint is None:
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	599 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	600 return None
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	601 # 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	602 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	603 offsetY = Point.distanceNorm2(closestPoint, p)
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	604 if offsetY < minOffsetY:
cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	605 minOffsetY = offsetY
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	606 snappedAlignmentIdx = alignmentIdx
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	607 snappedAlignmentLeadingPoint = alignment_p
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	608 snappedPoint = Point(closestPoint.x, closestPoint.y)
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	609 #Jump loop if significantly close
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	610 if offsetY < goodEnoughAlignmentDistance:
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	611 break
780 1b22d81ef5ff cleaned and checked storage with functions for curvilinear trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 776 diff changeset	612
573 cae4e5f3fe9f fixed and simplified getSYfromXY Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 571 diff changeset	613 #Get sub-segment distance
574 e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	614 if minOffsetY != float('inf'):
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	615 subsegmentDistance = Point.distanceNorm2(snappedPoint, alignments[snappedAlignmentIdx][snappedAlignmentLeadingPoint])
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	616 #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	617 alignmentDistanceS = alignments[snappedAlignmentIdx].getCumulativeDistance(snappedAlignmentLeadingPoint) + subsegmentDistance
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	618 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	619 offsetVector = p-snappedPoint
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	620 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	621 minOffsetY = -minOffsetY
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	622 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	623 else:
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	624 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	625 return None
e24eeb244698 first implementation of projection to curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 573 diff changeset	626
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	627 def getXYfromSY(s, y, alignmentNum, alignments):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	628 ''' Find X,Y coordinate from S,Y data.
568 538fb47b3007 minor modification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	629 if mode = 0 : return Snapped X,Y
538fb47b3007 minor modification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	630 if mode !=0 : return Real X,Y
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	631 '''
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	632 alignment = alignments[alignmentNum]
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	633 i = 1
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	634 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	635 i += 1
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	636 if i < len(alignment):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	637 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	638 #Get difference vector and then snap
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	639 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	640 normalizedV = dv.normalize()
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	641 #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	642 # add offset finally
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	643 orthoNormalizedV = normalizedV.orthogonal()
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	644 return alignment[i-1] + normalizedVd + orthoNormalizedVy
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	645 else:
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	646 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	647 return None
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	648
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	649
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	650 class NormAngle(object):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	651 '''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	652
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	653 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	654 self.norm = norm
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	655 self.angle = angle
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	656
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	657 @staticmethod
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	658 def fromPoint(p):
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	659 norm = p.norm2()
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	660 if norm > 0:
945 05d4302bf67e working motion pattern prediction with rotation and features Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 943 diff changeset	661 angle = p.angle()
339 9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	662 else:
9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	663 angle = 0.
250 59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	664 return NormAngle(norm, angle)
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	665
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	666 def __add__(self, other):
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	667 '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	668 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	669
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	670 def getPoint(self):
255 13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	671 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	672
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	673
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	674 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	675 '''Predicts the position in nTimeSteps at constant speed/acceleration'''
939 a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	676 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	677
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	678 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	679 '''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	680 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	681 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	682 predictedSpeedTheta = speedOrientation+control
928 063d1267585d work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 921 diff changeset	683 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	684 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	685 predictedPosition = position+predictedSpeedTheta.getPoint()
59f547aebaac modified prediction functions, added norm/angle representation of Points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 249 diff changeset	686 return predictedPosition, predictedSpeedTheta
245 bd8ab323c198 corrected issue with predictPosiont static method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 244 diff changeset	687
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	688
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	689 class FlowVector(object):
184 d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	690 '''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	691 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	692 def __init__(self, position, velocity):
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	693 '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	694 self.position = position
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	695 self.velocity = velocity
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	696
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	697 def __add__(self, other):
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	698 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	699
939 a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	700 def __mul__(self, alpha):
a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	701 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	702
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	703 def plot(self, options = '', **kwargs):
211 ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	704 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	705 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	706
184 d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	707 @staticmethod
d70e9b36889c initial work on flow vectors and clustering algorithms Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 182 diff changeset	708 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	709 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	710
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	711 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	712 ''' 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	713 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	714
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	715 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	716 dp12 = p2-p1
571 a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	717 dp34 = p4-p3
a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	718 #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	719 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	720 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	721 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	722 else:
571 a9c1d61a89b4 corrected bug for segment intersection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 570 diff changeset	723 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	724 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	725
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	726 # def intersection(p1, p2, dp1, dp2):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	727 # '''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	728 # 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	729 # from numpy import matrix
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	730 # from numpy.linalg import linalg
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	731 # A = matrix([[dp1.y, -dp1.x],
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	732 # [dp2.y, -dp2.x]])
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	733 # 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	734 # [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	735
569 0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	736 # if linalg.det(A) == 0:
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	737 # return None
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	738 # else:
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	739 # intersection = linalg.solve(A,B)
0057c04f94d5 work in progress on intersections (for PET) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 567 diff changeset	740 # 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	741
152 74b1fc68d4df re-organized code to avoid cyclic python module dependencies Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 148 diff changeset	742 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	743 '''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	744
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	745 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	746 return None, None
152 74b1fc68d4df re-organized code to avoid cyclic python module dependencies Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 148 diff changeset	747 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	748 inter, ratio = intersection(p1, p2, p3, p4)
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	749 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	750 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	751 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	752 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	753 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	754 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	755 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	756 return None, None
258 d90be3c02267 reasonably efficient computation of collision points and crossing zones Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 256 diff changeset	757
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	758 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	759 '''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	760 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	761 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	762 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	763 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	764 return None, None
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	765
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	766 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	767 '''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	768 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	769 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	770 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	771
290 df58d361f19e refactoring of Interval and TimeInterval using class methods (intersection, union) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 288 diff changeset	772 class Trajectory(object):
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	773 '''Class for trajectories: temporal sequence of positions
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	774
288 e0d41c7f53d4 updated class/method descriptions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 284 diff changeset	775 The class is iterable'''
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	776
79 5d487f183fe2 added method to test points in polygons and tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 74 diff changeset	777 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	778 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	779 self.positions = positions
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	780 else:
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	781 self.positions = [[],[]]
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	782
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	783 @staticmethod
555 f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	784 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	785 t = Trajectory()
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	786 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	787 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	788 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	789 p0 += v
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	790 t.addPosition(p0)
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	791 return t
555 f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	792
f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	793 @staticmethod
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	794 def load(line1, line2):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	795 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	796 [float(n) for n in line2.split(' ')]])
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	797
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	798 @staticmethod
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	799 def fromPointList(points):
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	800 t = Trajectory()
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	801 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	802 for p in points:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	803 t.addPositionXY(p[0],p[1])
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	804 else:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	805 for p in points:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	806 t.addPosition(p)
256 dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	807 return t
dc1faa7287bd added the normal adaptation class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 255 diff changeset	808
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	809 def __len__(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	810 return len(self.positions[0])
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	811
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	812 def length(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	813 return self.__len__()
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	814
582 7e1ae4d97f1a corrected bug for curvilinear trajectory with only one position and differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 581 diff changeset	815 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	816 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	817
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	818 def __getitem__(self, i):
1209 2064e52019db work on TTC for BEV Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1207 diff changeset	819 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	820 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	821 elif isinstance(i, slice):
3a06007a4bb7 modularized save trajectories, added slice to Trajectory, etc Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 904 diff changeset	822 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	823 else:
978 184f1dd307f9 corrected print and exception statements for Python 3 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 959 diff changeset	824 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	825
16 9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	826 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	827 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	828
69 cc192d0450b3 added full support for two implementations of indicators, with tests Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 68 diff changeset	829 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	830 return self.__str__()
23 5f2921ad4f7e made Trajectory indexable and timeinterval iterable Nicolas Saunier <nico@confins.net> parents: 22 diff changeset	831
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	832 def __iter__(self):
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	833 self.iterInstantNum = 0
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	834 return self
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	835
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	836 def __next__(self):
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	837 if self.iterInstantNum >= self.length():
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	838 raise StopIteration
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	839 else:
169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	840 self.iterInstantNum += 1
23 5f2921ad4f7e made Trajectory indexable and timeinterval iterable Nicolas Saunier <nico@confins.net> parents: 22 diff changeset	841 return self[self.iterInstantNum-1]
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	842
776 84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	843 def __eq__(self, other):
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	844 if self.length() == other.length():
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	845 result = True
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	846 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	847 result = result and (p == po)
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	848 return result
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	849 else:
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	850 return False
84420159c5f4 added __eq__ functions for Point and Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 771 diff changeset	851
1122 58efbe18f102 slight modification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1117 diff changeset	852 def append(self,other):
58efbe18f102 slight modification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1117 diff changeset	853 '''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	854 for i in range(2):
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	855 self.positions[i] += other.positions[i]
1122 58efbe18f102 slight modification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1117 diff changeset	856
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	857 def setPositionXY(self, i, x, y):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	858 if i < self.__len__():
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	859 self.positions[0][i] = x
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	860 self.positions[1][i] = y
203 e2f31813ade6 added code to display trajectories on videa Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 185 diff changeset	861
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	862 def setPosition(self, i, p):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	863 self.setPositionXY(i, p.x, p.y)
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	864
71 45e958ccd9bd added new addPosition method to Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 69 diff changeset	865 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	866 self.positions[0].append(x)
606010d1d9a4 corrected errors in trajectories (if empty) and getTrajectoryInPolygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 108 diff changeset	867 self.positions[1].append(y)
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	868
71 45e958ccd9bd added new addPosition method to Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 69 diff changeset	869 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	870 self.addPositionXY(p.x, p.y)
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	871
343 74e437ab5f11 first version of indicator loading code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 339 diff changeset	872 def duplicateLastPosition(self):
74e437ab5f11 first version of indicator loading code Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 339 diff changeset	873 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	874 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	875
182 d3f6de6c3918 added drawing functialities, in particular on aerial image Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 152 diff changeset	876 @staticmethod
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	877 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	878 if lastCoordinate is None:
270 05c9b0cb8202 updates for drawing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 268 diff changeset	879 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	880 elif 0 <= lastCoordinate <= len(positions[0]):
270 05c9b0cb8202 updates for drawing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 268 diff changeset	881 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	882 if withOrigin:
211 ada6e8fbe4c6 2 Changes : Francois Belisle <belisle.francois@gmail.com> parents: 203 diff changeset	883 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	884 if objNum is not None:
07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	885 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	886
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 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	888 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	889
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 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	891 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	892
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	893 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	894 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	895
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	896 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	897 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	898
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	899 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	900 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	901 [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	902 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	903
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	904 def getXCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	905 return self.positions[0]
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	906
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	907 def getYCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	908 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	909
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 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	911 return array(self.positions)
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	912
14 e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	913 def xBounds(self):
e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	914 # 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	915 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	916
14 e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	917 def yBounds(self):
e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	918 # 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	919 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	920
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	921 def add(self, traj2):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	922 '''Returns a new trajectory of the same length'''
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	923 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	924 print('Trajectories of different lengths')
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	925 return None
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	926 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	927 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	928 [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	929
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	930 def subtract(self, traj2):
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	931 '''Returns a new trajectory of the same length'''
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	932 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	933 print('Trajectories of different lengths')
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	934 return None
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	935 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	936 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	937 [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	938
939 a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	939 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	940 '''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	941 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	942 [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	943
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	944 def differentiate(self, doubleLastPosition = False):
339 9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	945 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	946 for i in range(1, self.length()):
339 9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	947 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	948 if doubleLastPosition:
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	949 diff.addPosition(diff[-1])
339 9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	950 return diff
9c1818a71c9c minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 335 diff changeset	951
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	952 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	953 '''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	954 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	955 Warning: high order polynomials yield artefacts
761 15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	956
15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	957 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	958 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	959 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	960 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	961 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	962 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	963 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	964
877 d1ff6917d082 added savitzky golay filter for accelerations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 869 diff changeset	965 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	966 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	967 length = self.length()
1181 b3b1a5dfa17c correct bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1180 diff changeset	968 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	969 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	970 n = int(round(length/2))-1
1195 27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	971 else:
27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	972 n = nInstantsIgnoredAtEnds
27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	973 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	974 return Trajectory(filtered.tolist())
761 15ddc8715236 added savitzky golay for differentiation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 727 diff changeset	975
16 9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	976 def norm(self):
9d6831cfe675 added tools for speed Nicolas Saunier <nico@confins.net> parents: 14 diff changeset	977 '''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	978 return hypot(self.positions[0], self.positions[1])
14 e7bbe8465591 homography and other utils Nicolas Saunier <nico@confins.net> parents: 13 diff changeset	979
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	980 def computeCumulativeDistances(self):
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	981 '''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	982 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	983 self.distances = []
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	984 self.cumulativeDistances = [0.]
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	985 p1 = self[0]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	986 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	987 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	988 p2 = self[i+1]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	989 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	990 cumulativeDistance += self.distances[-1]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	991 self.cumulativeDistances.append(cumulativeDistance)
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	992 p1 = p2
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	993
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	994 def getDistance(self,i):
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	995 '''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	996 if i < self.length()-1:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	997 return self.distances[i]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	998 else:
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	999 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	1000
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	1001 def getCumulativeDistance(self, i):
1115 cef7aa2f9931 minor shorthand method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1114 diff changeset	1002 '''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	1003 if i < self.length():
576 0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	1004 return self.cumulativeDistances[i]
0eff0471f9cb added functions to use trajectories as alignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 575 diff changeset	1005 else:
577 d0abd2ee17b9 changed arguments to type Point Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 576 diff changeset	1006 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	1007
1115 cef7aa2f9931 minor shorthand method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1114 diff changeset	1008 def getTotalDistance(self):
cef7aa2f9931 minor shorthand method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1114 diff changeset	1009 '''Returns the total distance (shorthand for cumulative distance [-1]'''
cef7aa2f9931 minor shorthand method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1114 diff changeset	1010 return self.getCumulativeDistance(-1)
cef7aa2f9931 minor shorthand method Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1114 diff changeset	1011
762 d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1012 def getMaxDistance(self, metric):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1013 '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	1014 positions = self.getPositions().asArray().T
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1015 return cdist(positions, positions, metric = metric).max()
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1016
937 b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1017 def getClosestPoint(self, p1, maxDist2 = None):
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1018 '''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	1019 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	1020 TODO: could use cdist for different metrics'''
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1021 distances2 = []
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1022 minDist2 = float('inf')
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1023 i = -1
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1024 for p2 in self:
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1025 distances2.append(Point.distanceNorm2(p1, p2))
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1026 if distances2[-1] < minDist2:
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1027 minDist2 = distances2[-1]
939 a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	1028 i = len(distances2)-1
a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	1029 if maxDist2 is not None and minDist2 < maxDist2:
a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	1030 return None
a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	1031 else:
937 b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1032 return i
b67a784beb69 work started on prototype prediction Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 936 diff changeset	1033
381 387cc0142211 script to replay event annotations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 379 diff changeset	1034 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	1035 '''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	1036 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	1037 count = 0
383 0ce2210790b1 fixed stupid naming bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 381 diff changeset	1038 lengthThreshold = float(self.length())*minProportion
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1039 for p in self:
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1040 if p.similarOrientation(refDirection, cosineThreshold):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1041 count += 1
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1042 return count >= lengthThreshold
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1043
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	1044 def wiggliness(self):
762 d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1045 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	1046 if straightDistance > 0:
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1047 return self.getCumulativeDistance(self.length()-1)/float(straightDistance)
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1048 else:
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1049 return None
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	1050
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	1051 def getIntersections(self, p1, p2, computeOrientations = False):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1052 '''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	1053 intersects with the segment of extremities p1 and p2
771 bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	1054 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	1055 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	1056 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	1057 rightToLeftOrientations = []
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1058
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	1059 for i in range(self.length()-1):
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1060 q1=self.__getitem__(i)
41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1061 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	1062 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	1063 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	1064 # 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	1065 # 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	1066 # 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	1067 # 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	1068 # 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	1069 # ratio = 0
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1070 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	1071 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	1072 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	1073 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	1074 return indices, intersections, rightToLeftOrientations
83 41da2cdcd91c re-arranged trajectory intersections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 79 diff changeset	1075
630 69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	1076 def getLineIntersections(self, p1, p2):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1077 '''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	1078 intersects with the line going through p1 and p2
771 bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	1079 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	1080 indices = []
69a98f84f3eb corrected major issue with pPET, only for CVDirect prediction for now Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 627 diff changeset	1081 intersections = []
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1082
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	1083 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	1084 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	1085 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	1086 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	1087 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	1088 # 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	1089 # 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	1090 # 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	1091 # 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	1092 # 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	1093 # 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	1094 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	1095 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	1096 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	1097
1109 6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1098 def subTrajectoryInInterval(self, inter):
622 dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	1099 '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	1100 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	1101 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	1102 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	1103 else:
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1104 return None
762 d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1105
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1106 def subSample(self, step):
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1107 'Returns the positions very step'
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1108 return Trajectory([self.positions[0][::step],
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1109 self.positions[1][::step]])
d6f0e0cab07d added functionalities for Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 761 diff changeset	1110
372 349eb1e09f45 Cleaned the methods/functions indicating if a point is in a polygon Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 369 diff changeset	1111 if shapelyAvailable:
863 a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1112 def getInstantsInPolygon(self, polygon):
a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1113 '''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	1114 instants = []
a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1115 n = self.length()
a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1116 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	1117 if polygon.contains(shapelyPoint(x, y)):
a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1118 instants.append(t)
a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1119 return instants
a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1120
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	1121 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	1122 '''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	1123 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	1124
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1125 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	1126 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	1127 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	1128 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	1129 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	1130 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	1131 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	1132 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	1133 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	1134 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	1135 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	1136 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	1137 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	1138 return traj, traj2
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1139
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1140 def proportionInPolygon(self, polygon, minProportion = 0.5):
863 a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1141 instants = self.getInstantsInPolygon(polygon)
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1142 lengthThreshold = float(self.length())*minProportion
863 a8ca72dc1564 work on user detectors Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 851 diff changeset	1143 return len(instants) >= lengthThreshold
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1144 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	1145 def getTrajectoryInPolygon(self, polygon, t2 = None):
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1146 '''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	1147 (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	1148 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	1149 inPolygon = []
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1150 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	1151 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	1152 if inPolygon[-1]:
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1153 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	1154 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	1155 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	1156 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	1157 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	1158 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	1159 return traj, traj2
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1160
82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1161 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	1162 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	1163 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	1164 return sum(inPolygon) >= lengthThreshold
627 82e9f78a4714 added test for location for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 626 diff changeset	1165
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	1166 @staticmethod
369 027e254f0b53 lcss subclass for indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 368 diff changeset	1167 def lcss(t1, t2, lcss):
027e254f0b53 lcss subclass for indicators Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 368 diff changeset	1168 return lcss.compute(t1, t2)
284 f2cf16ad798f added LCSS for trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 280 diff changeset	1169
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1170 class CurvilinearTrajectory(Trajectory):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1171 '''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	1172 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	1173 lateral coordinate is stored as second coordinate
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1174 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	1175 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	1176 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	1177
526 21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	1178 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	1179 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	1180 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	1181 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	1182 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	1183 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	1184 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	1185 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	1186 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	1187 else:
21bdeb29f855 corrected bug in initialization of lists and loading trajectories from vissim files Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 524 diff changeset	1188 self.lanes = lanes
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1189
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	1190 @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	1191 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	1192 '''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	1193 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	1194 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	1195 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	1196 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	1197 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	1198 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	1199 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	1200 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	1201
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1202 @staticmethod
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1203 def fromTrajectoryProjection(t, alignments, halfWidth = 3):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1204 ''' 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	1205 (curvilinearPositions instance) which holds information about the
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1206 curvilinear coordinates using alignment metadata.
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1207 From Paul St-Aubin's PVA tools
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
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1210 Input:
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1211 ======
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1212 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	1213 points (class Point).
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1214 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	1215 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	1216 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	1217
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1218 '''
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1219 curvilinearPositions = CurvilinearTrajectory()
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1220
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1221 #For each point
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1222 for i in range(int(t.length())):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1223 result = getSYfromXY(t[i], alignments)
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1224
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1225 # Error handling
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1226 if(result is None):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1227 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	1228 else:
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1229 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1230 curvilinearPositions.addPositionSYL(S, Y, align)
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1231
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1232 ## 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	1233 #Run through objects looking for outlier point
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1234 smoothed_lanes = utils.filterCategoricalMovingWindow(curvilinearPositions.getLanes(), halfWidth)
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1235 ## Recalculate projected point to new lane
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1236 lanes = curvilinearPositions.getLanes()
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1237 if(lanes != smoothed_lanes):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1238 for i in range(len(lanes)):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1239 if(lanes[i] != smoothed_lanes[i]):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1240 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	1241
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1242 # Error handling
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1243 if(result is None):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1244 ## 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	1245 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	1246 else:
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1247 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1248 curvilinearPositions.setPosition(i, S, Y, align)
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1249 return curvilinearPositions
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1250
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1251 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	1252 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	1253 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	1254 else:
978 184f1dd307f9 corrected print and exception statements for Python 3 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 959 diff changeset	1255 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	1256 #elif isinstance( key, slice ):
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1257
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1258 def getSCoordinates(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1259 return self.getXCoordinates()
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1260
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1261 def getLanes(self):
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1262 return self.lanes
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1263
1097 b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1264 def getSCoordAt(self, i):
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1265 return self.positions[0][i]
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1266
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1267 def getYCoordAt(self, i):
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1268 return self.positions[1][i]
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1269
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1270 def getLaneAt(self, i):
1098 469e36eea158 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1097 diff changeset	1271 return self.lanes[i]
1097 b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1272
1109 6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1273 def subTrajectoryInInterval(self, inter):
6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1274 '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	1275 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	1276 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	1277 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	1278 self.lanes[inter.first:inter.last+1])
6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1279 else:
6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1280 return None
6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1281
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1282 def append(self, other):
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1283 Trajectory.append(self, other)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1284 self.lanes.append(other.getLanes())
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1285
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	1286 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	1287 self.addPositionXY(s,y)
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1288 self.lanes.append(lane)
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1289
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	1290 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	1291 '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	1292 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	1293
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1294 def duplicateLastPosition(self):
6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1295 super(CurvilinearTrajectory, self).duplicateLastPosition()
6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1296 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	1297
327 42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1298 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	1299 self.setPositionXY(i, s, y)
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1300 if i < self.__len__():
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1301 self.lanes[i] = lane
42f2b46ec210 added class for trajectories in curvilinear coordinates Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 318 diff changeset	1302
542 a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	1303 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	1304 diff = CurvilinearTrajectory()
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	1305 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	1306 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	1307 p2 = self[i]
1098 469e36eea158 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1097 diff changeset	1308 if p2[2] == p1[2]:
469e36eea158 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1097 diff changeset	1309 laneChange = None
469e36eea158 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1097 diff changeset	1310 else:
469e36eea158 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1097 diff changeset	1311 laneChange = (p1[2], p2[2])
469e36eea158 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1097 diff changeset	1312 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	1313 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	1314 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	1315 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	1316 return diff
a3add9f751ef added differentiate function for curvilinear trajectories and modified the addPosition functions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 531 diff changeset	1317
451 cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	1318 def getIntersections(self, S1, lane = None):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1319 '''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	1320 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	1321 (in provided lane if lane is not None)
771 bd13937818a4 minor bug fix Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 768 diff changeset	1322 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	1323 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	1324 for i in range(self.length()-1):
451 cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	1325 q1=self.__getitem__(i)
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	1326 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	1327 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	1328 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	1329 return indices
cd342a774806 Point/CurvilinearTrajectory/Interaction utiles Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 411 diff changeset	1330
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1331 ##################
1210 1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1332 # Geometry
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
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1335 class Geometry(object):
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1336 '''Generic class for outline and size of object '''
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
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1339 class CarGeometry(Geometry):
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1340 '''Default car geometry as rectangle '''
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1341
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1342 ##################
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1343 # Moving Objects
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1344 ##################
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1345
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	1346 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	1347 'car',
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1348 '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	1349 'motorcycle',
a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	1350 'bicycle',
185 c06379f25ab8 utilities for user types Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 184 diff changeset	1351 'bus',
1125 b358bed29ab4 updates and bugs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1117 diff changeset	1352 'truck',
b358bed29ab4 updates and bugs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1117 diff changeset	1353 'automated']
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	1354
185 c06379f25ab8 utilities for user types Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 184 diff changeset	1355 userType2Num = utils.inverseEnumeration(userTypeNames)
c06379f25ab8 utilities for user types Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 184 diff changeset	1356
959 4f32d82ca390 corrected error due to change in Hog (scikit image) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 956 diff changeset	1357 class CarClassifier:
4f32d82ca390 corrected error due to change in Hog (scikit image) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 956 diff changeset	1358 def predict(self, hog):
4f32d82ca390 corrected error due to change in Hog (scikit image) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 956 diff changeset	1359 return userType2Num['car']
4f32d82ca390 corrected error due to change in Hog (scikit image) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 956 diff changeset	1360 carClassifier = CarClassifier()
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1361
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	1362 class MovingObject(STObject, VideoFilenameAddable):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1363 '''Class for moving objects: a spatio-temporal object
9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1364 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	1365 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	1366 '''
ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	1367
1203 7b3384a8e409 second version of code loading kitti data, to clean Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1197 diff changeset	1368 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	1369 super(MovingObject, self).__init__(num, timeInterval)
1097 b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1370 if initCurvilinear:
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1371 self.curvilinearPositions = positions
1098 469e36eea158 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1097 diff changeset	1372 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	1373 else:
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1374 self.positions = positions
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1375 self.velocities = velocities
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	1376 self.geometry = geometry
54 c354d41ef7cd corrected code for usertype in movingobject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 49 diff changeset	1377 self.userType = userType
1041 fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1378 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	1379 self.features = features
7 ffddccfab7f9 loading shell objects from NGSIM works Nicolas Saunier <nico@confins.net> parents: 6 diff changeset	1380 # 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	1381
1075 67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1382 @staticmethod
1079 845d694af7b8 reversed minor bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1077 diff changeset	1383 def croppedTimeInterval(obj, value, after = True):
1075 67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1384 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	1385 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	1386 newPositions = obj.positions[slice(newTimeInterval.first - obj.getLastInstant(), newTimeInterval.last + 1 - obj.getLastInstant())]
1075 67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1387 else:
67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1388 newPositions = None
67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1389 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	1390 newVelocities = obj.velocities[slice(newTimeInterval.first - obj.getLastInstant(), newTimeInterval.last + 1 - obj.getLastInstant())]
1075 67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1391 else:
67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1392 newVelocities = None
1079 845d694af7b8 reversed minor bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1077 diff changeset	1393 if obj.hasFeatures():
845d694af7b8 reversed minor bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1077 diff changeset	1394 newFeatures = [f.croppedTimeInterval(value, after) for f in obj.features]
1075 67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1395 else:
67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1396 newFeatures = None
67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1397 res = MovingObject(obj.getNum(), newTimeInterval, newPositions, newVelocities, obj.geometry, obj.userType, obj.nObjects)
67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1398 res.features = newFeatures
67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1399 res.featureNumbers = obj.featureNumbers
1079 845d694af7b8 reversed minor bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1077 diff changeset	1400 #if hasattr(obj, 'projectedPositions'):
845d694af7b8 reversed minor bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1077 diff changeset	1401 # res.projectedPositions = obj.projectedPositions[slice(newTimeInterval.first, newTimeInterval.last+1)]
1075 67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1402 return res
67144f26609e Updates crop Wendlasida parents: 1044 diff changeset	1403
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	1404
555 f13220f765e0 added static methods to create trajectories and moving objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 552 diff changeset	1405 @staticmethod
1019 5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1406 def aggregateTrajectories(features, interval = None, aggFunc = mean):
995 349cd5e73f79 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 994 diff changeset	1407 '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	1408 positions = Trajectory()
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1409 velocities = Trajectory()
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1410 if interval is None:
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1411 inter = TimeInterval.unionIntervals([f.getTimeInterval() for f in features])
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1412 else:
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1413 inter = interval
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1414 for t in inter:
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1415 points = []
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1416 vels = []
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1417 for f in features:
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1418 if f.existsAtInstant(t):
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1419 points.append(f.getPositionAtInstant(t))
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1420 vels.append(f.getVelocityAtInstant(t))
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1421 positions.addPosition(Point.agg(points, aggFunc))
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1422 velocities.addPosition(Point.agg(vels, aggFunc))
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1423 return inter, positions, velocities
995 349cd5e73f79 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 994 diff changeset	1424
349cd5e73f79 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 994 diff changeset	1425 @staticmethod
918 3a06007a4bb7 modularized save trajectories, added slice to Trajectory, etc Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 904 diff changeset	1426 def generate(num, p, v, timeInterval):
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1427 nPoints = int(timeInterval.length())
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1428 positions = Trajectory.generate(p, v, nPoints)
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1429 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	1430
1019 5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1431 def updatePositions(self):
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1432 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	1433
622 dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	1434 @staticmethod
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1435 def concatenate(obj1, obj2, num = None, newFeatureNum = None):
1018 d7afc59f6966 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1016 diff changeset	1436 '''Concatenates two objects, whether overlapping temporally or not
d7afc59f6966 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1016 diff changeset	1437
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1438 Positions will be recomputed if features are merged
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1439 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	1440 if num is None:
4f3387a242a1 updated utils to python 3 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 996 diff changeset	1441 newNum = obj1.getNum()
4f3387a242a1 updated utils to python 3 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 996 diff changeset	1442 else:
4f3387a242a1 updated utils to python 3 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 996 diff changeset	1443 newNum = num
623 ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1444 commonTimeInterval = obj1.commonTimeInterval(obj2)
1180 dc28364f34b9 minor cleanup Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1179 diff changeset	1445 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	1446 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	1447 if newFeatureNum is None:
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1448 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	1449 return None, None
1018 d7afc59f6966 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1016 diff changeset	1450 else:
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1451 if obj1.existsAtInstant(emptyInterval.last):
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1452 firstObject = obj2
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1453 secondObject = obj1
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1454 else:
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1455 firstObject = obj1
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1456 secondObject = obj2
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1457 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	1458 positions = copy.deepcopy(firstObject.getPositions())
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1459 velocities = copy.deepcopy(firstObject.getPositions())
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1460 featurePositions = Trajectory()
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1461 featureVelocities = Trajectory()
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1462 p = firstObject.getPositionAtInstant(emptyInterval.first)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1463 # 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	1464 featurePositions.addPosition(p)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1465 featureVelocities.addPosition(v)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1466 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	1467 p=p+v
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1468 positions.addPosition(p)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1469 velocities.addPosition(v)
1134 4b2a55d570c1 resolve issue for short features when merging objects manually Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1133 diff changeset	1470 featurePositions.addPosition(p)
4b2a55d570c1 resolve issue for short features when merging objects manually Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1133 diff changeset	1471 featureVelocities.addPosition(v)
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1472 # last position to feature
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1473 p = secondObject.getPositionAtInstant(emptyInterval.last)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1474 v = secondObject.getVelocityAtInstant(emptyInterval.last)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1475 featurePositions.addPosition(p)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1476 featureVelocities.addPosition(v)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1477 # copy second trajectory
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1478 positions.append(secondObject.getPositions())
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1479 velocities.append(secondObject.getVelocities())
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1480 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	1481 newFeature = MovingObject(newFeatureNum, emptyInterval, featurePositions, featureVelocities)
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1482 if hasattr(obj1, 'featureNumbers') and hasattr(obj2, 'featureNumbers'):
1019 5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1483 newObject.featureNumbers = obj1.featureNumbers+obj2.featureNumbers+[newFeatureNum]
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1484 if obj1.hasFeatures() and obj2.hasFeatures():
aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1485 newObject.features = obj1.getFeatures()+obj2.getFeatures()+[newFeature]
1019 5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1486 else: # time intervals overlap
623 ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1487 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	1488 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	1489 newFeature = None
1019 5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1490 if hasattr(obj1, 'featureNumbers') and hasattr(obj2, 'featureNumbers'):
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1491 newObject.featureNumbers = obj1.featureNumbers+obj2.featureNumbers
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1492 if obj1.hasFeatures() and obj2.hasFeatures():
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1493 newObject.features = obj1.getFeatures()+obj2.getFeatures()
5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1494 newObject.updatePositions()
623 ce7133cbcdf3 implemented function to concatenate objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 622 diff changeset	1495 else:
1019 5d2f6afae35b work on object concatenation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1018 diff changeset	1496 print('Cannot update object positions without features')
1018 d7afc59f6966 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1016 diff changeset	1497 # user type
d7afc59f6966 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1016 diff changeset	1498 if obj1.getUserType() != obj2.getUserType():
d7afc59f6966 work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1016 diff changeset	1499 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	1500 newObject.setUserType(obj1.getUserType())
1177 aa88acf06876 rewrote object concatenation method, cleaner Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1170 diff changeset	1501 return newObject, newFeature
622 dc8490726d06 corrected issues created with homography projection in Trajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 621 diff changeset	1502
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1503 def getObjectInTimeInterval(self, inter):
54 c354d41ef7cd corrected code for usertype in movingobject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 49 diff changeset	1504 '''Returns a new object extracted from self,
c354d41ef7cd corrected code for usertype in movingobject Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 49 diff changeset	1505 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	1506 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	1507 if not intersection.empty():
b3a1c26e2f22 corrected getObjectInInterval for MovingObject and timeintervals for TemporalIndicator Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 96 diff changeset	1508 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	1509 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	1510 if self.velocities is not None:
1146 b219d5a1bb55 added code to categorize interactions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1136 diff changeset	1511 obj.velocities = self.velocities.subTrajectoryInInterval(trajectoryInterval)
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1512 return obj
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1513 else:
978 184f1dd307f9 corrected print and exception statements for Python 3 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 959 diff changeset	1514 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	1515 return None
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	1516
726 43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1517 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	1518 '''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	1519 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	1520 if homography is not None:
936 56cc8a1f7082 removed all old versions of projection methods Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 935 diff changeset	1521 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	1522 else:
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1523 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	1524 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	1525 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	1526 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	1527
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1528 #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	1529 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	1530 # '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	1531 l = 0
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1532 intervalLabels = []
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1533 prev = True
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1534 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	1535 if i:
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1536 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	1537 l += 1
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1538 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	1539 else:
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1540 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	1541 prev = i
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1542 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	1543 subObjects = []
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1544 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	1545 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	1546 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	1547 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	1548 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	1549 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	1550
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1551 return subObjects
43ae3a1af290 added functionality to display matchings between ground truth and tracked objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 723 diff changeset	1552
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1553 def getPositions(self):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1554 return self.positions
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1555
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1556 def getVelocities(self):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1557 return self.velocities
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1558
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	1559 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	1560 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	1561
943 b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 939 diff changeset	1562 def computeCumulativeDistances(self):
b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 939 diff changeset	1563 self.positions.computeCumulativeDistances()
b1e8453c207c work on motion prediction using motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 939 diff changeset	1564
335 3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1565 def getCurvilinearPositions(self):
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1566 if hasattr(self, 'curvilinearPositions'):
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1567 return self.curvilinearPositions
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1568 else:
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1569 return None
3950bfe22768 added functions to export trajectories to csv Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 331 diff changeset	1570
1123 0548a78852b8 added accessor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1122 diff changeset	1571 def getCurvilinearVelocities(self):
0548a78852b8 added accessor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1122 diff changeset	1572 if hasattr(self, 'curvilinearVelocities'):
0548a78852b8 added accessor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1122 diff changeset	1573 return self.curvilinearVelocities
0548a78852b8 added accessor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1122 diff changeset	1574 else:
0548a78852b8 added accessor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1122 diff changeset	1575 return None
0548a78852b8 added accessor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1122 diff changeset	1576
643 bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1577 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	1578 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	1579 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	1580 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	1581 if withOrigin:
994dd644f6ab corrected impact of warmUpLastInstant Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 659 diff changeset	1582 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	1583 else:
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1584 instants = []
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1585 coords = []
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1586 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	1587 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	1588 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	1589 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	1590 else:
bfaa6b95dae2 added function to plot curvilinear position as a function of time Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 636 diff changeset	1591 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	1592 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	1593 plot(instants, coords, options, **kwargs)
660 994dd644f6ab corrected impact of warmUpLastInstant Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 659 diff changeset	1594 if withOrigin and len(instants)>0:
994dd644f6ab corrected impact of warmUpLastInstant Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 659 diff changeset	1595 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	1596 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	1597 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	1598
1116 a3982d591a61 placeholder implementation for interpolateCurvilinearPositions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1115 diff changeset	1599 def interpolateCurvilinearPositions(self, t, alignments = None):
1097 b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1600 '''Linear interpolation of curvilinear positions, t being a float'''
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1601 if hasattr(self, 'curvilinearPositions'):
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1602 if self.existsAtInstant(t):
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1603 i = int(floor(t))
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1604 p1 = self.getCurvilinearPositionAtInstant(i)
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1605 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	1606 if p1[2] == p2[2]:
7135b5eaa6b4 correcting poor requirement for interpolateCurvilinearPositions (when changing alignment) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1112 diff changeset	1607 alpha = t-float(i)
7135b5eaa6b4 correcting poor requirement for interpolateCurvilinearPositions (when changing alignment) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1112 diff changeset	1608 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	1609 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	1610 pass # TODO
1097 b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1611 else:
1116 a3982d591a61 placeholder implementation for interpolateCurvilinearPositions Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1115 diff changeset	1612 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	1613 else:
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1614 print('Object {} does not exist at {}'.format(self.getNum(), t))
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1615 else:
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1616 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	1617
329 a70c205ebdd9 added sqlite code, in particular to load and save road user type Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 327 diff changeset	1618 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	1619 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	1620
1041 fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1621 def getNObjects(self):
fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1622 return self.nObjects
fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1623
fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1624 def setNObjects(self, nObjects):
fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1625 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	1626 self.nObjects = nObjects
fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1627 else:
fc7c0f38e8a6 added nObjects to MovingObject, with loading/saving Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1037 diff changeset	1628 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	1629 self.nObjects = None
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1630
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	1631 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	1632 '''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	1633 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	1634 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	1635 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	1636 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	1637 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	1638 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	1639
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	1640 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	1641 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	1642
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 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	1644 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	1645
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 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	1647 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	1648 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	1649 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	1650 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	1651
869 eb2f8ce2b39d added method for longest feature Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 863 diff changeset	1652 def getNLongestFeatures(self, nFeatures = 1):
eb2f8ce2b39d added method for longest feature Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 863 diff changeset	1653 if self.features is None:
eb2f8ce2b39d added method for longest feature Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 863 diff changeset	1654 return []
eb2f8ce2b39d added method for longest feature Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 863 diff changeset	1655 else:
eb2f8ce2b39d added method for longest feature Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 863 diff changeset	1656 tmp = utils.sortByLength(self.getFeatures(), reverse = True)
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1657 return tmp[:min(len(tmp), nFeatures)]
9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1658
1044 75a6ad604cc5 work on motion patterns Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1041 diff changeset	1659 def getFeatureNumbersOverTime(self):
666 93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1660 '''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	1661 dict instant -> number of features'''
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1662 if self.hasFeatures():
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1663 featureNumbers = {}
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1664 for t in self.getTimeInterval():
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1665 n = 0
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1666 for f in self.getFeatures():
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1667 if f.existsAtInstant(t):
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1668 n += 1
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1669 featureNumbers[t]=n
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1670 return featureNumbers
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1671 else:
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1672 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	1673 return None
93633ce122c3 added function to count features in objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 665 diff changeset	1674
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1675 def getSpeeds(self, nInstantsIgnoredAtEnds = 0):
1197 0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1676 '''Returns the scalar speed'''
682 fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1677 speeds = self.getVelocities().norm()
fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1678 if nInstantsIgnoredAtEnds > 0:
fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1679 n = min(nInstantsIgnoredAtEnds, int(floor(self.length()/2.)))
fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1680 return speeds[n:-n]
fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1681 else:
fbe29be25501 corrected bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 680 diff changeset	1682 return speeds
49 1fb5606506ae re-arrangement Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 43 diff changeset	1683
1197 0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1684 def getAccelerations(self, fromSpeeds = True, nInstantsIgnoredAtEnds = 0):
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1685 '''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	1686 if fromSpeeds:
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1687 speeds = array(self.getSpeeds())
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1688 accelerations = speeds[1:]-speeds[:-1]
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1689 else:
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1690 accelerations = self.getVelocities().differentiate().norm()
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1691 if nInstantsIgnoredAtEnds > 0:
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1692 n = min(nInstantsIgnoredAtEnds, int(floor(len(accelerations)/2.)))
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1693 return accelerations[n:-n]
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1694 else:
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1695 return accelerations
0475b4cd0cfb adding simple direct acceleration calculation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1196 diff changeset	1696
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	1697 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	1698 '''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	1699 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	1700 speeds = self.getSpeeds()
1195 27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	1701 wlength = min(window_length, len(speeds))
27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	1702 if wlength % 2 == 0:
27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	1703 wlength -=1
1193 d324305c1240 solving issues with extreme values when filtering using savitsky golay Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1190 diff changeset	1704 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	1705 if nInstantsIgnoredAtEnds >= 1:
1195 27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	1706 if nInstantsIgnoredAtEnds >= len(speeds)/2:
27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	1707 n = int(round(len(speeds)/2))-1
27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	1708 else:
27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	1709 n = nInstantsIgnoredAtEnds
27a6a7f9b972 adjusting savgol use Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1193 diff changeset	1710 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	1711 else:
d324305c1240 solving issues with extreme values when filtering using savitsky golay Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1190 diff changeset	1712 return filtered
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1713
633 f410c8fb07b7 minor function to export speed as temporal indicator for display Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 631 diff changeset	1714 def getSpeedIndicator(self):
f410c8fb07b7 minor function to export speed as temporal indicator for display Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 631 diff changeset	1715 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	1716 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	1717
38 0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1718 def getPositionAt(self, i):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1719 return self.positions[i]
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1720
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1721 def getVelocityAt(self, i):
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1722 return self.velocities[i]
0d321c23d337 added norm functions for Point and accessor methods for MovingObject Nicolas Saunier <nico@confins.net> parents: 30 diff changeset	1723
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1724 def getPositionAtInstant(self, i):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1725 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	1726
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1727 def getVelocityAtInstant(self, i):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	1728 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	1729
1097 b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1730 def getCurvilinearPositionAt(self, i):
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1731 return self.curvilinearPositions[i]
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1732
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1733 def getCurvilinearVelocityAt(self, i):
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1734 return self.curvilinearVelocities[i]
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1735
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1736 def getCurvilinearPositionAtInstant(self, i):
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1737 return self.curvilinearPositions[i-self.getFirstInstant()]
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1738
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1739 def getCurvilinearVelocityAtInstant(self, i):
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1740 return self.curvilinearVelocities[i-self.getFirstInstant()]
b3f8b26ee838 modification for simulation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1096 diff changeset	1741
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1742 def getXCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1743 return self.positions.getXCoordinates()
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1744
21 3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1745 def getYCoordinates(self):
3c4629550f5f added basic getters for objects Nicolas Saunier <nico@confins.net> parents: 19 diff changeset	1746 return self.positions.getYCoordinates()
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1747
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1748 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	1749 if withIds:
07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1750 objNum = self.getNum()
07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1751 else:
07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1752 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	1753 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	1754 for f in self.getFeatures():
517 47d9970ee954 added plotting of features with object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 516 diff changeset	1755 f.positions.plot('r', True, timeStep, **kwargs)
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1756 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	1757 else:
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1758 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	1759
851 07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1760 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	1761 if withIds:
07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1762 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	1763 else:
07fb949ff98f added display of object id Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 845 diff changeset	1764 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, None, **kwargs)
22 169cd4679366 made Trajectory iterable Nicolas Saunier <nico@confins.net> parents: 21 diff changeset	1765
1210 1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1766 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	1767 if self.hasFeatures():
7b3384a8e409 second version of code loading kitti data, to clean Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1197 diff changeset	1768 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	1769 Point.plotAll(points, True, options, **kwargs)
1210 1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1770 if withVelocity:
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1771 p = self.getPositionAtInstant(t)
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1772 v = self.getVelocityAtInstant(t)*velocityMultiply
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1773 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	1774
626 35155ac2a294 corrected bugs, in particular to MovingObject.play Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 624 diff changeset	1775 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	1776 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	1777
877 d1ff6917d082 added savitzky golay filter for accelerations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 869 diff changeset	1778 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	1779 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	1780 coef = utils.linearRegression(list(range(len(speeds))), speeds)
379 f1a1923ddff9 minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 378 diff changeset	1781 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	1782 if display:
672 5473b7460375 moved and rationalized imports in modules Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 666 diff changeset	1783 from matplotlib.pyplot import figure, axis
377 2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1784 figure(1)
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	1785 self.plot()
379 f1a1923ddff9 minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 378 diff changeset	1786 axis('equal')
377 2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1787 figure(2)
2aed569f39e7 added utils Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 372 diff changeset	1788 plot(list(self.getTimeInterval()), speeds)
877 d1ff6917d082 added savitzky golay filter for accelerations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 869 diff changeset	1789 figure(3)
d1ff6917d082 added savitzky golay filter for accelerations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 869 diff changeset	1790 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	1791
378 3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1792 @staticmethod
653 107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1793 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	1794 '''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	1795 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	1796 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	1797 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	1798 maxDistance = minDistance
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1799 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	1800 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	1801 if d<minDistance:
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1802 minDistance = d
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1803 elif d>maxDistance:
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1804 maxDistance = d
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1805 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	1806 else:
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1807 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	1808
107f1ad02b69 compilation with C++11 and test function for movingobject distance Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 643 diff changeset	1809 @staticmethod
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1810 def distances(obj1, obj2, instant1, _instant2 = None):
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1811 '''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	1812 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	1813 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	1814 if _instant2 is None:
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1815 instant2 = instant1
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1816 else:
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1817 instant2 = _instant2
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1818 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	1819 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	1820 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	1821
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1822 @staticmethod
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1823 def minDistance(obj1, obj2, instant1, instant2 = None):
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1824 return MovingObject.distances(obj1, obj2, instant1, instant2).min()
546 6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1825
6c0923f1ce68 add some functions for behaviour analysis MohamedGomaa parents: 531 diff changeset	1826 @staticmethod
551 dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1827 def maxDistance(obj1, obj2, instant, instant2 = None):
dc3739ac2371 rearranged all distance computations between points Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 550 diff changeset	1828 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	1829
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1830 def maxSize(self):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1831 '''Returns the max distance between features
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1832 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	1833 if hasattr(self, 'features'):
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1834 nFeatures = -1
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1835 tMaxFeatures = 0
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1836 for t in self.getTimeInterval():
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1837 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	1838 if n > nFeatures:
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1839 nFeatures = n
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1840 tMaxFeatures = t
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1841 return MovingObject.maxDistance(self, self, tMaxFeatures)
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1842 else:
3805b9639647 added tests for movement orientation, object size Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 377 diff changeset	1843 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	1844 return None
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1845
550 5668af2ff515 minor naming Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 549 diff changeset	1846 def setRoutes(self, startRouteID, endRouteID):
5668af2ff515 minor naming Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 549 diff changeset	1847 self.startRouteID = startRouteID
5668af2ff515 minor naming Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 549 diff changeset	1848 self.endRouteID = endRouteID
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1849
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	1850 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	1851 '''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	1852 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	1853 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	1854 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	1855 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	1856
949 d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	1857 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	1858 '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	1859 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	1860 self.prototypeSimilarities = []
d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	1861 for proto in prototypes:
d6c1c05d11f5 modified multithreading at the interaction level for safety computations Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 945 diff changeset	1862 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	1863 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	1864 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	1865
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	1866 @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	1867 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	1868 '''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	1869
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	1870 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	1871 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	1872 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	1873 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	1874 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	1875 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	1876 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	1877 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	1878 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	1879 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	1880 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	1881 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	1882 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	1883 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	1884 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	1885 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	1886 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	1887 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	1888 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	1889
244 5027c174ab90 moved indicators to new file, added ExtrapolatedTrajectory class to extrapolation file Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 243 diff changeset	1890 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	1891 '''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	1892 at constant speed'''
255 13ec22bec5d4 corrected typos and bugs and added a test Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 250 diff changeset	1893 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	1894
1086 8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1895 def projectCurvilinear(self, alignments, halfWidth = 3):
8734742c08c0 major refactoring of curvilinear trajectory projections Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1085 diff changeset	1896 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	1897
562 259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1898 def computeSmoothTrajectory(self, minCommonIntervalLength):
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1899 '''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	1900 if a feature exists, its position is
9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	1901
567 072cedc3f33d first integration of curvilinear transformation from Paul Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 562 diff changeset	1902 Warning work in progress
562 259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1903 TODO? not use the first/last 1-.. positions'''
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1904 nFeatures = len(self.features)
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1905 if nFeatures == 0:
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1906 print('Empty object features\nCannot compute smooth trajectory')
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1907 else:
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1908 # compute the relative position vectors
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1909 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	1910 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	1911 for j in range(i):
562 259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1912 fi = self.features[i]
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1913 fj = self.features[j]
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1914 inter = fi.commonTimeInterval(fj)
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1915 if inter.length() >= minCommonIntervalLength:
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1916 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	1917 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	1918 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	1919 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	1920 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	1921 relativePositions[(j,i)] = -relativePositions[(i,j)]
259ccb3dd962 work on object trajectory smoothing Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 555 diff changeset	1922
1212 af329f3330ba work in progress on 3D safety analysis Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1210 diff changeset	1923 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	1924 '''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	1925 bounding box format is a list of points
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1926 shape is the type of output shape:
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1927 - raw means the list of feature positions
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1928 - 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	1929 if self.hasFeatures():
1210 1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1930 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	1931 if shape == 'rect':
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1932 return Point.boundingRectangle(positions, self.getVelocityAtInstant(t))
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1933 elif shape == 'raw':
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1934 return positions
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1935 else:
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1936 print('Unknown shape')
1bad5f9b60de work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1209 diff changeset	1937 return None
1106 799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	1938 else:
799ef82caa1a added code to compute bounding rectangle around each user Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1100 diff changeset	1939 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	1940 return None
1109 6e8ab471ebd4 minor adjustment to Lionel s needs Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1106 diff changeset	1941
1132 09ef0dc994a0 modification for performance computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1126 diff changeset	1942 def motDistanceAtInstant(self, obj, instant):
09ef0dc994a0 modification for performance computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1126 diff changeset	1943 '''Returns distance for computing CLEAR MOT metrics
09ef0dc994a0 modification for performance computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1126 diff changeset	1944 (returns an actual value, otherwise munkres does not terminate)'''
09ef0dc994a0 modification for performance computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1126 diff changeset	1945 return Point.distanceNorm2(self.getPositionAtInstant(instant), obj.getPositionAtInstant(instant))
09ef0dc994a0 modification for performance computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1126 diff changeset	1946
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	1947 ###
ce40a89bd6ae added 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 # 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	1949 ###
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1950 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	1951 '''Classifies slow and fast road users
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1952 slow: non-motorized -> pedestrians
607 84690dfe5560 add some functions for behaviour analysis MohamedGomaa parents: 583 diff changeset	1953 fast: motorized -> cars
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	1954
608 078adacd72a4 moving.py integrating Mohamed's comments refactored Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 607 diff changeset	1955 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	1956 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	1957 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	1958 if aggregationFunc(speeds) >= threshold:
345 fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1959 self.setUserType(userType2Num['car'])
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1960 else:
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1961 self.setUserType(userType2Num['pedestrian'])
fa64b2e3a64f added simple classification based on speed Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 343 diff changeset	1962
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1963 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	1964 '''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	1965 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	1966 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	1967 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	1968
ce40a89bd6ae added 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 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	1970 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	1971 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	1972 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	1973 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	1974 return x'''
626 35155ac2a294 corrected bugs, in particular to MovingObject.play Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 624 diff changeset	1975 if not hasattr(self, 'aggregatedSpeed'):
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1976 self.aggregatedSpeed = aggregationFunc(self.getSpeeds(nInstantsIgnoredAtEnds))
581 10e8a9f2bd9f change for python 2.6 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 579 diff changeset	1977 userTypeProbabilities = {}
10e8a9f2bd9f change for python 2.6 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 579 diff changeset	1978 for userTypename in speedProbabilities:
10e8a9f2bd9f change for python 2.6 Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 579 diff changeset	1979 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	1980 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	1981 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	1982
928 063d1267585d work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 921 diff changeset	1983 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	1984 '''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	1985
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1986 TODO? compute speed for longest feature?'''
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1987 self.aggregatedSpeed = aggregationFunc(self.getSpeeds(nInstantsIgnoredAtEnds))
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1988 if self.aggregatedSpeed < pedBikeSpeedTreshold or bikeCarSVM is None:
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1989 self.appearanceClassifier = pedBikeCarSVM
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1990 elif self.aggregatedSpeed < bikeCarSpeedThreshold:
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1991 self.appearanceClassifier = bikeCarSVM
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	1992 else:
959 4f32d82ca390 corrected error due to change in Hog (scikit image) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 956 diff changeset	1993 self.appearanceClassifier = carClassifier
928 063d1267585d work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 921 diff changeset	1994 # project feature positions
063d1267585d work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 921 diff changeset	1995 if self.hasFeatures():
063d1267585d work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 921 diff changeset	1996 for f in self.getFeatures():
935 0e63a918a1ca updated classify-objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 933 diff changeset	1997 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	1998 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	1999 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	2000
929 be28a3538dc9 work in progress on projection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 928 diff changeset	2001 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	2002 '''Extracts the image box around the object
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	2003 (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	2004 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	2005 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	2006 and applies the SVM model on it'''
929 be28a3538dc9 work in progress on projection Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 928 diff changeset	2007 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	2008 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	2009 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	2010 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	2011 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	2012 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	2013
898 1fc901d983ed better take into account unknown appearance classification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 893 diff changeset	2014 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	2015 '''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	2016 (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	2017
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	2018 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	2019 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	2020
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	2021 Considered categories are the keys of speedProbabilities'''
da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	2022 if not hasattr(self, 'aggregatedSpeed') or not hasattr(self, 'userTypes'):
845 aa98e773ac91 minor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 828 diff changeset	2023 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	2024 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	2025
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	2026 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	2027 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	2028 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	2029 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	2030 # compute P(Speed\|Class)
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	2031 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	2032 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	2033 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	2034 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	2035 # compute P(Class\|Appearance)
680 da1352b89d02 classification is working Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 675 diff changeset	2036 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	2037 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	2038 for t in self.userTypes:
898 1fc901d983ed better take into account unknown appearance classification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 893 diff changeset	2039 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	2040 # 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	2041 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	2042 for userTypeNum in userTypeProbabilities:
1fc901d983ed better take into account unknown appearance classification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 893 diff changeset	2043 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	2044 # 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	2045 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	2046 self.setUserType(userType2Num['unknown'])
1fc901d983ed better take into account unknown appearance classification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 893 diff changeset	2047 else:
1fc901d983ed better take into account unknown appearance classification Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 893 diff changeset	2048 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	2049
ce40a89bd6ae added 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 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	2051 '''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	2052 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	2053 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	2054
ce40a89bd6ae added 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 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	2056 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	2057 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	2058 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	2059 if homography is not None:
936 56cc8a1f7082 removed all old versions of projection methods Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 935 diff changeset	2060 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	2061 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	2062 self.projectedPositions = obj.positions
523 ce4eaabacc26 modified internal implementation of user type for classifyUserTypeArea Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 522 diff changeset	2063 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	2064 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	2065 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	2066 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	2067 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	2068 # 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	2069 return possibleUserTypes
520 fd9641cbd24b added function to classify object at instant from SVM Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 517 diff changeset	2070
67 ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	2071 @staticmethod
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	2072 def collisionCourseDotProduct(movingObject1, movingObject2, instant):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	2073 '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	2074 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	2075 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	2076 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	2077
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	2078 @staticmethod
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	2079 def collisionCourseCosine(movingObject1, movingObject2, instant):
ded58c424783 added indicator computation and modified severity indicator constructor Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 66 diff changeset	2080 '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	2081 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	2082 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	2083
587 cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	2084
921 630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2085 class Prototype(object):
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2086 'Class for a prototype'
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2087
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2088 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	2089 self.filename = filename
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2090 self.num = num
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2091 self.trajectoryType = trajectoryType
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2092 self.nMatchings = nMatchings
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2093 self.movingObject = None
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2094
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2095 def getFilename(self):
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2096 return self.filename
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2097 def getNum(self):
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2098 return self.num
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2099 def getTrajectoryType(self):
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2100 return self.trajectoryType
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2101 def getNMatchings(self):
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2102 return self.nMatchings
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2103 def getMovingObject(self):
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2104 return self.movingObject
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2105 def setMovingObject(self, o):
630934595871 work in progress with prototype class Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 918 diff changeset	2106 self.movingObject = o
1035 933588568bec major update to learn motion pattern, see program description Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1030 diff changeset	2107 def __str__(self):
933588568bec major update to learn motion pattern, see program description Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1030 diff changeset	2108 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	2109 def __eq__(self, p2):
6a6c37eb3a74 added function to load prototype assignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1035 diff changeset	2110 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	2111 def __hash__(self):
6a6c37eb3a74 added function to load prototype assignments Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1035 diff changeset	2112 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	2113
587 cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	2114 ##################
588 c5406edbcf12 added loading ground truth annotations (ground truth) from polytrack format Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 587 diff changeset	2115 # Annotations
587 cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	2116 ##################
cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	2117
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	2118 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	2119 '''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	2120 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	2121 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	2122
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	2123 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	2124
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	2125 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	2126 (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	2127 '''
587 cf578ba866da added code to load bounding box corners as trajectories Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 583 diff changeset	2128
1132 09ef0dc994a0 modification for performance computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1126 diff changeset	2129 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	2130 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	2131 self.topLeftPositions = topLeftPositions.getPositions()
5800a87f11ae corrected one bug and changed attribute names Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 588 diff changeset	2132 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	2133
0fa73cbe9fdb annotation centroid are computed explicitly and projected at that step if required Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 589 diff changeset	2134 def computeCentroidTrajectory(self, homography = None):
939 a2f3f3ca241e work in progress Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 937 diff changeset	2135 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	2136 if homography is not None:
936 56cc8a1f7082 removed all old versions of projection methods Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 935 diff changeset	2137 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	2138
1081 346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2139 def matchObjects(obj1, obj2, instant, matchingDistance):
346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2140 '''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	2141 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	2142 (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	2143 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	2144 if d < matchingDistance:
346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2145 return d
346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2146 else:
346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2147 return matchingDistance + 1
346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2148
346b41cbc81a added code idea from Paul St Aubin Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1079 diff changeset	2149 # 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	2150 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	2151 '''Computes the CLEAR MOT metrics
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2152
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2153 Reference:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2154 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	2155
1190 d24d57e4de24 work on optimization Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1185 diff changeset	2156 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	2157 current implementation is BBMovingObject (bounding boxes)
592 985a3021cff2 first match table implementation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 590 diff changeset	2158 mathingDistance is threshold on matching between annotation and object
985a3021cff2 first match table implementation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 590 diff changeset	2159
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2160 TO: tracker output (objects)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2161 GT: ground truth (annotations)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2162
624 bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	2163 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	2164 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	2165 mme number of mismatches
bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	2166 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	2167 gt number of GT.frames
bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	2168
723 e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	2169 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	2170 matches is a dict
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	2171 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	2172 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	2173 (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	2174
624 bac66bd536c5 added slight documentation of CLEAR MOT output Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 623 diff changeset	2175 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	2176 (add argument useDistanceForWeights = False)'''
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2177 from munkres import Munkres
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	2178
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2179 munk = Munkres()
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2180 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	2181 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	2182 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	2183 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	2184 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	2185 mme = 0 # number of mismatches
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2186 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	2187 if returnMatches:
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	2188 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	2189 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	2190 else:
196a1fd498ba removing unnecessary complexity of varying number of return elements Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 949 diff changeset	2191 gtMatches = None
196a1fd498ba removing unnecessary complexity of varying number of return elements Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 949 diff changeset	2192 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	2193 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	2194 previousMatches = matches.copy()
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2195 # 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	2196 toDelete = []
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2197 for a in matches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2198 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	2199 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	2200 if d < matchingDistance:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2201 dist += d
592 985a3021cff2 first match table implementation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 590 diff changeset	2202 else:
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2203 toDelete.append(a)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2204 else:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2205 toDelete.append(a)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2206 for a in toDelete:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2207 del matches[a]
593 e2a873e08568 non-working clear mot metrics Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 592 diff changeset	2208
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2209 # 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	2210 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	2211 matchedTOs = list(matches.values())
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2212 costs = []
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2213 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	2214 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	2215 nGTs = len(matchedGTs)+len(unmatchedGTs)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2216 nTOs = len(matchedTOs)+len(unmatchedTOs)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2217 if len(unmatchedTOs) > 0:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2218 for a in unmatchedGTs:
1133 c4d9c270f999 modification for performance computation Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1132 diff changeset	2219 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	2220 if len(costs) > 0:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2221 newMatches = munk.compute(costs)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2222 for k,v in newMatches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2223 if costs[k][v] < matchingDistance:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2224 matches[unmatchedGTs[k]]=unmatchedTOs[v]
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2225 dist += costs[k][v]
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2226 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	2227 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	2228 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	2229 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	2230 gtMatches[a.getNum()][t] = o.getNum()
e14e2101a5a9 returns detailed matching information for clear mot Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 690 diff changeset	2231 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	2232
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2233 # compute metrics elements
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2234 ct += len(matches)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2235 mt += nGTs-len(matches)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2236 fpt += nTOs-len(matches)
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2237 gt += nGTs
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2238 # compute mismatches
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2239 # 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	2240 mismatches = []
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2241 for a in matches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2242 if a in previousMatches:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2243 if matches[a] != previousMatches[a]:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2244 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	2245 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	2246 mismatches.append(matches[a])
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2247 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	2248 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	2249 mismatches.append(previousMatches[a])
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	2250 if debug:
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2251 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	2252 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	2253 # 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	2254 mme += len(set(mismatches))
1110 6bbcd9433732 formatting and addition of one method to CurvilinearTrajectory Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1109 diff changeset	2255
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2256 if ct > 0:
9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2257 motp = dist/ct
593 e2a873e08568 non-working clear mot metrics Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 592 diff changeset	2258 else:
594 9e39cd95e017 first implementation of CLEAR MOT (needs formal tests) Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 593 diff changeset	2259 motp = None
595 17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	2260 if gt > 0:
17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	2261 mota = 1.-float(mt+fpt+mme)/gt
17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	2262 else:
17b02c8054d0 added tests and corrected one bug Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 594 diff changeset	2263 mota = None
956 196a1fd498ba removing unnecessary complexity of varying number of return elements Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 949 diff changeset	2264 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	2265
62 290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	2266 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	2267 '''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	2268 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	2269 figure()
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	2270 for obj in objects:
515 727e3c529519 renamed all draw functions to plot for consistency Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 504 diff changeset	2271 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	2272 axis('equal')
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	2273
290fceb125d2 moved road user types and added plotting for all road users Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 58 diff changeset	2274
1016 9b53be469a36 getting ready for pull request Wendlasida parents: 1014 diff changeset	2275 if __name__ == "__main__":
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	2276 import doctest
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	2277 import unittest
43 6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	2278 suite = doctest.DocFileSuite('tests/moving.txt')
6d11d9e7ad4e methods for trajectories and objects Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 41 diff changeset	2279 #suite = doctest.DocTestSuite()
2 de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	2280 unittest.TextTestRunner().run(suite)
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	2281 #doctest.testmod()
de5642925615 started implementation of TimeInterval and Spatio-temporal object Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 0 diff changeset	2282 #doctest.testfile("example.txt")
1096 9a32d63bae3f update from Lionel Nebot Janvier Nicolas Saunier <nicolas.saunier@polymtl.ca> parents: 1094 diff changeset	2283 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	2284 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	2285 unittest.TextTestRunner().run(suite)

Mercurial Hosting > traffic-intelligence

annotate trafficintelligence/moving.py @ 1215:1b472cddf9b1