nsaunier/traffic-intelligence: python/moving.py comparison

comparison python/moving.py @ 795:a34ec862371f

merged with dev branch

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Mon, 09 May 2016 15:33:11 -0400
parents	3aa6102ccc12
children	21f10332c72b

comparison

equal deleted inserted replaced

-:0a05883216cf
+:a34ec862371f
 from scipy.stats import scoreatpercentile
 from scipy.spatial.distance import cdist
 try:
 from shapely.geometry import Polygon, Point as shapelyPoint
-from shapely.prepared import prep
+from shapely.prepared import prep, PreparedGeometry
 shapelyAvailable = True
 except ImportError:
 print('Shapely library could not be loaded')
 shapelyAvailable = False
 return '[{0}, {1}]'.format(self.first, self.last)
 def __repr__(self):
 return self.__str__()
+def __eq__(self, other):
+return ((self.first == other.first) and (self.last == other.last)) or ((self.first == other.last) and (self.last == other.first))
 def empty(self):
 return self.first > self.last
 def center(self):
 return (self.first+self.last)/2.
 return (self.first<=instant and self.last>=instant)
 def inside(self, interval2):
 '''Indicates if the temporal interval of self is comprised in interval2'''
 return (self.first >= interval2.first) and (self.last <= interval2.last)
+def shift(self, offset):
+self.first += offset
+self.last += offset
 @classmethod
 def union(cls, interval1, interval2):
 '''Smallest interval comprising self and interval2'''
 return cls(min(interval1.first, interval2.first), max(interval2.last, interval2.last))
 return self.timeInterval.contains(t)
 def commonTimeInterval(self, obj2):
 return TimeInterval.intersection(self.getTimeInterval(), obj2.getTimeInterval())
+def shiftTimeInterval(self, offset):
+self.timeInterval.shift(offset)
 class Point(object):
 def __init__(self, x, y):
 self.x = x
 self.y = y
 def __str__(self):
 return '({:f},{:f})'.format(self.x,self.y)
 def __repr__(self):
 return self.__str__()
+def __eq__(self, other):
+return (self.x == other.x) and (self.y == other.y)
 def __add__(self, other):
 return Point(self.x+other.x, self.y+other.y)
 def __sub__(self, other):
 'Warning, returns a new Point'
 return Point(self.x/alpha, self.y/alpha)
 def plot(self, options = 'o', **kwargs):
 plot([self.x], [self.y], options, **kwargs)
+@staticmethod
+def plotSegment(p1, p2, options = 'o', **kwargs):
+plot([p1.x, p2.x], [p1.y, p2.y], options, **kwargs)
 def norm2Squared(self):
 '''2-norm distance (Euclidean distance)'''
 return self.x**2+self.y**2
 'Returns the middle of the segment [p1, p2]'
 return Point(0.5*p1.x+0.5*p2.x, 0.5*p1.y+0.5*p2.y)
 if shapelyAvailable:
 def pointsInPolygon(points, polygon):
-'''Optimized tests of a series of points within (Shapely) polygon '''
+'''Optimized tests of a series of points within (Shapely) polygon (not prepared)'''
-prepared_polygon = prep(polygon)
+if type(polygon) == PreparedGeometry:
+prepared_polygon = polygon
+else:
+prepared_polygon = prep(polygon)
 return filter(prepared_polygon.contains, points)
 # Functions for coordinate transformation
 # From Paul St-Aubin's PVA tools
 def subsec_spline_dist(splines):
 mode=2: cumulative distance with trailing distance
 '''
 ss_spline_d = []
 #Prepare subsegment distances
 for spline in range(len(splines)):
-ss_spline_d.append([[],[],[]])
+ss_spline_d[spline]=[]#.append([[],[],[]])
-ss_spline_d[spline][0] = zeros(len(splines[spline])-1)  #Incremental distance
+ss_spline_d[spline].append(zeros(len(splines[spline])-1))  #Incremental distance
-ss_spline_d[spline][1] = zeros(len(splines[spline])-1)  #Cumulative distance
+ss_spline_d[spline].append(zeros(len(splines[spline])-1))  #Cumulative distance
-ss_spline_d[spline][2] = zeros(len(splines[spline]))  #Cumulative distance with trailing distance
+ss_spline_d[spline].append(zeros(len(splines[spline])))  #Cumulative distance with trailing distance
 for spline_p in range(len(splines[spline])):
 if spline_p > (len(splines[spline]) - 2):
 break
 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])
 ss_spline_d[spline][1][spline_p] = sum(ss_spline_d[spline][0][0:spline_p])
 ss_spline_d[spline][2][spline_p] = ss_spline_d[spline][1][spline_p]#sum(ss_spline_d[spline][0][0:spline_p])
 ss_spline_d[spline][2][-1] = ss_spline_d[spline][2][-2] + ss_spline_d[spline][0][-1]
 return ss_spline_d
+def prepareSplines(splines):
+'Approximates slope singularity by giving some slope roundoff; account for roundoff error'
+for spline in splines:
+p1 = spline[0]
+for i in xrange(len(spline)-1):
+p2 = spline[i+1]
+if(round(p1.x, 10) == round(p2.x, 10)):
+p2.x += 0.0000000001
+if(round(p1.y, 10) == round(p2.y, 10)):
+p2.y += 0.0000000001
+p1 = p2
 def ppldb2p(qx,qy, p0x,p0y, p1x,p1y):
 ''' Point-projection (Q) on line defined by 2 points (P0,P1).
 http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf
 '''
 if(p0x == p1x and p0y == p1y):
 return None
 try:
 #Approximate slope singularity by giving some slope roundoff; account for roundoff error
-if(round(p0x, 10) == round(p1x, 10)):
+# if(round(p0x, 10) == round(p1x, 10)):
-p1x += 0.0000000001
+#     p1x += 0.0000000001
-if(round(p0y, 10) == round(p1y, 10)):
+# if(round(p0y, 10) == round(p1y, 10)):
-p1y += 0.0000000001
+#     p1y += 0.0000000001
 #make the calculation
 Y = (-(qx)*(p0y-p1y)-(qy*(p0y-p1y)**2)/(p0x-p1x)+p0x**2*(p0y-p1y)/(p0x-p1x)-p0x*p1x*(p0y-p1y)/(p0x-p1x)-p0y*(p0x-p1x))/(p1x-p0x-(p0y-p1y)**2/(p0x-p1x))
 X = (-Y*(p1y-p0y)+qx*(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x)
 except ZeroDivisionError:
 print('Error: Division by zero in ppldb2p. Please report this error with the full traceback:')
 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y))
 import pdb; pdb.set_trace()
 return Point(X,Y)
 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5):
-''' Snap a point p to it's nearest subsegment of it's nearest spline (from the list splines). A spline is a list of points (class Point), most likely a trajectory.
+''' Snap a point p to it's nearest subsegment of it's nearest spline (from the list splines).
+A spline is a list of points (class Point), most likely a trajectory.
 Output:
 =======
 [spline index,
 subsegment leading point index,
 snapped point,
 or None
 '''
 minOffsetY = float('inf')
 #For each spline
-for spline in range(len(splines)):
+for splineIdx in range(len(splines)):
 #For each spline point index
-for spline_p in range(len(splines[spline])-1):
+for spline_p in range(len(splines[splineIdx])-1):
 #Get closest point on spline
-closestPoint = ppldb2p(p.x,p.y,splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][spline_p+1][0],splines[spline][spline_p+1][1])
+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])
 if closestPoint is None:
-print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p))
+print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(splineIdx, spline_p))
 return None
-# check if the
+# check if the projected point is in between the current segment of the alignment bounds
-if utils.inBetween(splines[spline][spline_p][0], splines[spline][spline_p+1][0], closestPoint.x) and utils.inBetween(splines[spline][spline_p][1], splines[spline][spline_p+1][1], closestPoint.y):
+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):
 offsetY = Point.distanceNorm2(closestPoint, p)
 if offsetY < minOffsetY:
 minOffsetY = offsetY
-snappedSpline = spline
+snappedSplineIdx = splineIdx
 snappedSplineLeadingPoint = spline_p
 snappedPoint = Point(closestPoint.x, closestPoint.y)
 #Jump loop if significantly close
 if offsetY < goodEnoughSplineDistance:
 break
 #Get sub-segment distance
 if minOffsetY != float('inf'):
-subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSpline][snappedSplineLeadingPoint])
+subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSplineIdx][snappedSplineLeadingPoint])
 #Get cumulative alignment distance (total segment distance)
-splineDistanceS = splines[snappedSpline].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance
+splineDistanceS = splines[snappedSplineIdx].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance
-orthogonalSplineVector = (splines[snappedSpline][snappedSplineLeadingPoint+1]-splines[snappedSpline][snappedSplineLeadingPoint]).orthogonal()
+orthogonalSplineVector = (splines[snappedSplineIdx][snappedSplineLeadingPoint+1]-splines[snappedSplineIdx][snappedSplineLeadingPoint]).orthogonal()
 offsetVector = p-snappedPoint
 if Point.dot(orthogonalSplineVector, offsetVector) < 0:
 minOffsetY = -minOffsetY
-return [snappedSpline, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY]
+return [snappedSplineIdx, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY]
 else:
+print('Offset for point {} is infinite (check with prepareSplines if some spline segments are aligned with axes)'.format(p))
 return None
 def getXYfromSY(s, y, splineNum, splines, mode = 0):
 ''' Find X,Y coordinate from S,Y data.
 if mode = 0 : return Snapped X,Y
 return ' '.join([self.__getitem__(i).__str__() for i in xrange(self.length())])
 def __repr__(self):
 return self.__str__()
 def __iter__(self):
 self.iterInstantNum = 0
 return self
 def next(self):
 if self.iterInstantNum >= self.length():
 raise StopIteration
 else:
 self.iterInstantNum += 1
 return self[self.iterInstantNum-1]
+def __eq__(self, other):
+if self.length() == other.length():
+result = True
+for p, po in zip(self, other):
+result = result and (p == po)
+return result
+else:
+return False
 def setPositionXY(self, i, x, y):
 if i < self.__len__():
 self.positions[0][i] = x
 self.positions[1][i] = y
 for i in xrange(1, self.length()):
 diff.addPosition(self[i]-self[i-1])
 if doubleLastPosition:
 diff.addPosition(diff[-1])
 return diff
+def differentiateSG(self, window_length, polyorder, deriv=0, delta=1.0, axis=-1, mode='interp', cval=0.0, removeBothEnds = 2):
+'''Differentiates the trajectory using the Savitsky Golay filter
+window_length : The length of the filter window (i.e. the number of coefficients). window_length must be a positive odd integer.
+polyorder : The order of the polynomial used to fit the samples. polyorder must be less than window_length.
+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.
+delta : The spacing of the samples to which the filter will be applied. This is only used if deriv > 0. Default is 1.0.
+axis : The axis of the array x along which the filter is to be applied. Default is -1.
+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.
+cval : Value to fill past the edges of the input if mode is constant. Default is 0.0.'''
+from scipy.signal import savgol_filter
+if removeBothEnds >=1:
+pos = [self.positions[0][removeBothEnds:-removeBothEnds],
+self.positions[1][removeBothEnds:-removeBothEnds]]
+else:
+pos = self.positions
+filtered = savgol_filter(pos, window_length, polyorder, deriv, delta, axis, mode, cval)
+return Trajectory(filtered)
 def norm(self):
 '''Returns the list of the norms at each instant'''
 #        def add(x, y): return x+y
 #        sq = map(add, [x*x for x in self.positions[0]], [y*y for y in self.positions[1]])
 if i < self.length():
 return self.cumulativeDistances[i]
 else:
 print('Index {} beyond trajectory length {}'.format(i, self.length()))
+def getMaxDistance(self, metric):
+'Returns the maximum distance between points in the trajectory'
+positions = self.getPositions().asArray().T
+return cdist(positions, positions, metric = metric).max()
 def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5):
 '''Indicates whether the minProportion (<=1.) (eg half) of the trajectory elements (vectors for velocity)
 have a cosine with refDirection is smaller than cosineThreshold'''
 count = 0
 lengthThreshold = float(self.length())*minProportion
 if p.similarOrientation(refDirection, cosineThreshold):
 count += 1
 return count >= lengthThreshold
 def wiggliness(self):
-return self.getCumulativeDistance(self.length()-1)/float(Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1)))
+straightDistance = Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1))
+if straightDistance > 0:
+return self.getCumulativeDistance(self.length()-1)/float(straightDistance)
+else:
+return None
 def getIntersections(self, p1, p2):
 '''Returns a list of the indices at which the trajectory
 intersects with the segment of extremities p1 and p2
-the list is empty if there is no crossing'''
+Returns an empty list if there is no crossing'''
 indices = []
 intersections = []
 for i in xrange(self.length()-1):
 q1=self.__getitem__(i)
 q2=self.__getitem__(i+1)
-p = utils.segmentIntersection(q1, q2, p1, p2)
+p = segmentIntersection(q1, q2, p1, p2)
-if p is not None:
-if q1.x != q2.x:
-ratio = (p.x-q1.x)/(q2.x-q1.x)
-elif q1.y != q2.y:
-ratio = (p.y-q1.y)/(q2.y-q1.y)
-else:
-ratio = 0
-indices.append(i+ratio)
-intersections.append(p)
-return indices
-def getLineIntersections(self, p1, p2):
-'''Returns a list of the indices at which the trajectory
-intersects with the segment of extremities p1 and p2
-the list is empty if there is no crossing'''
-indices = []
-intersections = []
-for i in xrange(self.length()-1):
-q1=self.__getitem__(i)
-q2=self.__getitem__(i+1)
-p = utils.segmentLineIntersection(p1, p2, q1, q2)
 if p is not None:
 if q1.x != q2.x:
 ratio = (p.x-q1.x)/(q2.x-q1.x)
 elif q1.y != q2.y:
 ratio = (p.y-q1.y)/(q2.y-q1.y)
 ratio = 0
 indices.append(i+ratio)
 intersections.append(p)
 return indices, intersections
+def getLineIntersections(self, p1, p2):
+'''Returns a list of the indices at which the trajectory
+intersects with the line going through p1 and p2
+Returns an empty list if there is no crossing'''
+indices = []
+intersections = []
+for i in xrange(self.length()-1):
+q1=self.__getitem__(i)
+q2=self.__getitem__(i+1)
+p = segmentLineIntersection(p1, p2, q1, q2)
+if p is not None:
+if q1.x != q2.x:
+ratio = (p.x-q1.x)/(q2.x-q1.x)
+elif q1.y != q2.y:
+ratio = (p.y-q1.y)/(q2.y-q1.y)
+else:
+ratio = 0
+indices.append(i+ratio)
+intersections.append(p)
+return indices, intersections
 def getTrajectoryInInterval(self, inter):
 'Returns all position between index inter.first and index.last (included)'
 if inter.first >=0 and inter.last<= self.length():
 return Trajectory([self.positions[0][inter.first:inter.last+1],
 self.positions[1][inter.first:inter.last+1]])
 else:
 return None
+def subSample(self, step):
+'Returns the positions very step'
+return Trajectory([self.positions[0][::step],
+self.positions[1][::step]])
 if shapelyAvailable:
-def getTrajectoryInPolygon(self, polygon):
+def getTrajectoryInPolygon(self, polygon, t2 = None):
-'''Returns the trajectory built with the set of points inside the (shapely) polygon'''
+'''Returns the trajectory built with the set of points inside the (shapely) polygon
+The polygon could be a prepared polygon (faster) from prepared.prep
+t2 is another trajectory (could be velocities)
+which is filtered based on the first (self) trajectory'''
 traj = Trajectory()
-points = [p.asShapely() for p in self]
+inPolygon = []
-for p in pointsInPolygon(points, polygon):
+for x, y in zip(self.positions[0], self.positions[1]):
-traj.addPositionXY(p.x, p.y)
+inPolygon.append(polygon.contains(shapelyPoint(x, y)))
-return traj
+if inPolygon[-1]:
+traj.addPositionXY(x, y)
+traj2 = Trajectory()
+if t2 is not None:
+for inp, x, y in zip(inPolygon, t2.positions[0], t2.positions[1]):
+if inp:
+traj2.addPositionXY(x, y)
+return traj, traj2
 def proportionInPolygon(self, polygon, minProportion = 0.5):
-pointsIn = pointsInPolygon([p.asShapely() for p in self], polygon)
+inPolygon = [polygon.contains(shapelyPoint(x, y)) for x, y in zip(self.positions[0], self.positions[1])]
 lengthThreshold = float(self.length())*minProportion
-return len(pointsIn) >= lengthThreshold
+return sum(inPolygon) >= lengthThreshold
 else:
-def getTrajectoryInPolygon(self, polygon):
+def getTrajectoryInPolygon(self, polygon, t2 = None):
 '''Returns the trajectory built with the set of points inside the polygon
 (array of Nx2 coordinates of the polygon vertices)'''
 traj = Trajectory()
+inPolygon = []
 for p in self:
-if p.inPolygon(polygon):
+inPolygon.append(p.inPolygon(polygon))
+if inPolygon[-1]:
 traj.addPosition(p)
-return traj
+traj2 = Trajectory()
+if t2 is not None:
+for inp, x, y in zip(inPolygon, t2.positions[0], t2.positions[1]):
+if inp:
+traj2.addPositionXY(p.x, p.y)
+return traj, traj2
 def proportionInPolygon(self, polygon, minProportion = 0.5):
-pointsInPolygon = [p.inPolygon(polygon) for p in self]
+inPolygon = [p.inPolygon(polygon) for p in self]
 lengthThreshold = float(self.length())*minProportion
-return len(pointsInPolygon) >= lengthThreshold
+return sum(inPolygon) >= lengthThreshold
 @staticmethod
 def lcss(t1, t2, lcss):
 return lcss.compute(t1, t2)
 def getIntersections(self, S1, lane = None):
 '''Returns a list of the indices at which the trajectory
 goes past the curvilinear coordinate S1
 (in provided lane if lane is not None)
-the list is empty if there is no crossing'''
+Returns an empty list if there is no crossing'''
 indices = []
 for i in xrange(self.length()-1):
 q1=self.__getitem__(i)
 q2=self.__getitem__(i+1)
 if q1[0] <= S1 < q2[0] and (lane is None or (self.lanes[i] == lane and self.lanes[i+1] == lane)):
 ##################
 # Annotations
 ##################
-class BBAnnotation(MovingObject):
+class BBMovingObject(MovingObject):
-'''Class for a series of ground truth annotations using bounding boxes
+'''Class for a moving object represented as a bounding box
-Its center is the center of the containing shape
+used for series of ground truth annotations using bounding boxes
+and for the output of Urban Tracker http://www.jpjodoin.com/urbantracker/
 By default in image space
+Its center is the center of the box (generalize to other shapes?)
+(computed after projecting if homography available)
 '''
 def __init__(self, num = None, timeInterval = None, topLeftPositions = None, bottomRightPositions = None, userType = userType2Num['unknown']):
-super(BBAnnotation, self).__init__(num, timeInterval, userType = userType)
+super(BBMovingObject, self).__init__(num, timeInterval, userType = userType)
 self.topLeftPositions = topLeftPositions.getPositions()
 self.bottomRightPositions = bottomRightPositions.getPositions()
 def computeCentroidTrajectory(self, homography = None):
 self.positions = self.topLeftPositions.add(self.bottomRightPositions).multiply(0.5)
 Reference:
 Keni, Bernardin, and Stiefelhagen Rainer. "Evaluating multiple object tracking performance: the CLEAR MOT metrics." EURASIP Journal on Image and Video Processing 2008 (2008)
 objects and annotations are supposed to in the same space
-current implementation is BBAnnotations (bounding boxes)
+current implementation is BBMovingObject (bounding boxes)
 mathingDistance is threshold on matching between annotation and object
 TO: tracker output (objects)
 GT: ground truth (annotations)

Mercurial > hg > nsaunier > traffic-intelligence

comparison python/moving.py @ 795:a34ec862371f