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

annotate trafficintelligence/moving.py @ 1028:cc5cb04b04b0

major update using the trafficintelligence package name and install through pip

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Fri, 15 Jun 2018 11:19:10 -0400
parents	python/moving.py@5d2f6afae35b
children	c6cf75a2ed08

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

Mercurial Hosting > traffic-intelligence

annotate trafficintelligence/moving.py @ 1028:cc5cb04b04b0