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

comparison trafficintelligence/moving.py @ 1110:6bbcd9433732

formatting and addition of one method to CurvilinearTrajectory

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Mon, 06 May 2019 16:16:28 -0400
parents	6e8ab471ebd4
children	345cd9cd62d8

comparison

equal deleted inserted replaced

-:6e8ab471ebd4
+:6bbcd9433732
 def plotSegment(p1, p2, options = 'o', **kwargs):
 plot([p1.x, p2.x], [p1.y, p2.y], options, **kwargs)
 def angle(self):
 return atan2(self.y, self.x)
 def norm2Squared(self):
 '''2-norm distance (Euclidean distance)'''
 return self.x**2+self.y**2
 def norm2(self):
 '''2-norm distance (Euclidean distance)'''
 return sqrt(self.norm2Squared())
 def norm1(self):
 return abs(self.x)+abs(self.y)
 def normMax(self):
 return max(abs(self.x),abs(self.y))
 def aslist(self):
 return [self.x, self.y]
 return p1.x*p2.y-p1.y*p2.x
 @staticmethod
 def parallel(p1, p2):
 return Point.cross(p1, p2) == 0.
 @staticmethod
 def cosine(p1, p2):
 return Point.dot(p1,p2)/(p1.norm2()*p2.norm2())
 @staticmethod
 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()
+import pdb;
-return Point(X,Y)
+pdb.set_trace()
+return Point(X, Y)
 def getSYfromXY(p, alignments, goodEnoughAlignmentDistance = 0.5):
 ''' Snap a point p to its nearest subsegment of it's nearest alignment (from the list alignments).
 A alignment is a list of points (class Point), most likely a trajectory.
 return alignment[i-1] + normalizedV*d + orthoNormalizedV*y
 else:
 print('Curvilinear point {} is past the end of the alignement'.format((s, y, alignmentNum)))
 return None
 class NormAngle(object):
 '''Alternate encoding of a point, by its norm and orientation'''
 def __init__(self, norm, angle):
 self.norm = norm
 self.angle = angle
 @staticmethod
 def fromPoint(p):
 norm = p.norm2()
 if norm > 0:
 angle = p.angle()
 return FlowVector(self.position.__mul__(alpha), self.velocity.__mul__(alpha))
 def plot(self, options = '', **kwargs):
 plot([self.position.x, self.position.x+self.velocity.x], [self.position.y, self.position.y+self.velocity.y], options, **kwargs)
 self.position.plot(options+'x', **kwargs)
 @staticmethod
 def similar(f1, f2, maxDistance2, maxDeltavelocity2):
 return (f1.position-f2.position).norm2Squared()<maxDistance2 and (f1.velocity-f2.velocity).norm2Squared()<maxDeltavelocity2
 def intersection(p1, p2, p3, p4):
 #     from numpy.linalg import linalg
 #     A = matrix([[dp1.y, -dp1.x],
 #                 [dp2.y, -dp2.x]])
 #     B = matrix([[dp1.y*p1.x-dp1.x*p1.y],
 #                 [dp2.y*p2.x-dp2.x*p2.y]])
 #     if linalg.det(A) == 0:
 #         return None
 #     else:
 #         intersection = linalg.solve(A,B)
 #         return Point(intersection[0,0], intersection[1,0])
 inter = intersection(p1, p2, p3, p4)
 if inter is not None and utils.inBetween(p3.x, p4.x, inter.x) and utils.inBetween(p3.y, p4.y, inter.y):
 return inter
 else:
 return None
 class Trajectory(object):
 '''Class for trajectories: temporal sequence of positions
 The class is iterable'''
 def getYCoordinates(self):
 return self.positions[1]
 def asArray(self):
 return array(self.positions)
 def xBounds(self):
 # look for function that does min and max in one pass
 return Interval(min(self.getXCoordinates()), max(self.getXCoordinates()))
 def yBounds(self):
 # look for function that does min and max in one pass
 return Interval(min(self.getYCoordinates()), max(self.getYCoordinates()))
 def add(self, traj2):
 '''Returns a new trajectory of the same length'''
 if self.length() != traj2.length():
 print('Trajectories of different lengths')
 return None
 '''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 range(self.length()-1):
 q1=self.__getitem__(i)
 q2=self.__getitem__(i+1)
 p = segmentLineIntersection(p1, p2, q1, q2)
 if p is not None:
 self.positions = [S,Y]
 if lanes is None or len(lanes) != self.length():
 self.lanes = [None]*int(self.length())
 else:
 self.lanes = lanes
 @staticmethod
 def generate(s, v, nPoints, lane, y = 0):
 '''s is initial position, v is velocity
 0 in lateral coordinate by default
 TODO 2D velocity for lane change?'''
 raise TypeError("Invalid argument type.")
 #elif isinstance( key, slice ):
 def getSCoordinates(self):
 return self.getXCoordinates()
 def getLanes(self):
 return self.lanes
 def getSCoordAt(self, i):
 return self.positions[0][i]
 self.lanes.append(lane)
 def addPosition(self, p):
 'Adds position in the point format for curvilinear of list with 3 values'
 self.addPositionSYL(p[0], p[1], p[2])
+def duplicateLastPosition(self):
+super(CurvilinearTrajectory, self).duplicateLastPosition()
+self.lanes.append(self.lanes[-1])
 def setPosition(self, i, s, y, lane):
 self.setPositionXY(i, s, y)
 if i < self.__len__():
 self.lanes[i] = lane
 class CarClassifier:
 def predict(self, hog):
 return userType2Num['car']
 carClassifier = CarClassifier()
 class MovingObject(STObject, VideoFilenameAddable):
 '''Class for moving objects: a spatio-temporal object
 with a trajectory and a geometry (constant volume over time)
 and a usertype (e.g. road user) coded as a number (see userTypeNames)
 '''
 self.geometry = geometry
 self.userType = userType
 self.setNObjects(nObjects) # a feature has None for nObjects
 self.features = None
 # compute bounding polygon from trajectory
 @staticmethod
 def croppedTimeInterval(obj, value, after = True):
 newTimeInterval = TimeInterval(obj.getFirstInstant(), min(value, obj.getLastInstant())) if after else TimeInterval(max(obj.getFirstInstant(), value), obj.getLastInstant())
 if obj.positions is not None :
 newPositions = obj.positions[slice(newTimeInterval.first, newTimeInterval.last+1)]
 return [(1-alpha)*p1[0]+alpha*p2[0], (1-alpha)*p1[1]+alpha*p2[1], lane]
 else:
 print('Object {} does not exist at {}'.format(self.getNum(), t))
 else:
 print('Object {} has no curvilinear positions'.format(self.getNum()))
 def setUserType(self, userType):
 self.userType = userType
 def getNObjects(self):
 return self.nObjects
 if nObjects is None or nObjects >= 1:
 self.nObjects = nObjects
 else:
 print('Number of objects represented by object {} must be greater or equal to 1 ({})'.format(self.getNum(), nObjects))
 self.nObjects = None
 def setFeatures(self, features, featuresOrdered = False):
 '''Sets the features in the features field based on featureNumbers
 if not all features are loaded from 0, one needs to renumber in a dict'''
 if featuresOrdered:
 tmp = features
 '''Returns the 1-D acceleration from the 1-D speeds
 Caution about previously filtered data'''
 if speeds is None:
 speeds = self.getSpeeds(nInstantsIgnoredAtEnds)
 return savgol_filter(speeds, window_length, polyorder, 1, delta, axis, mode, cval)
 def getSpeedIndicator(self):
 from indicators import SeverityIndicator
 return SeverityIndicator('Speed', {t:self.getVelocityAtInstant(t).norm2() for t in self.getTimeInterval()})
 def getPositionAt(self, i):
 def getCurvilinearVelocityAtInstant(self, i):
 return self.curvilinearVelocities[i-self.getFirstInstant()]
 def getXCoordinates(self):
 return self.positions.getXCoordinates()
 def getYCoordinates(self):
 return self.positions.getYCoordinates()
 def plot(self, options = '', withOrigin = False, timeStep = 1, withFeatures = False, withIds = False, **kwargs):
 if withIds:
 objNum = self.getNum()
 else:
 objNum = None
 else:
 instant2 = _instant2
 positions1 = [f.getPositionAtInstant(instant1).astuple() for f in obj1.features if f.existsAtInstant(instant1)]
 positions2 = [f.getPositionAtInstant(instant2).astuple() for f in obj2.features if f.existsAtInstant(instant2)]
 return cdist(positions1, positions2, metric = 'euclidean')
 @staticmethod
 def minDistance(obj1, obj2, instant1, instant2 = None):
 return MovingObject.distances(obj1, obj2, instant1, instant2).min()
 @staticmethod
 tMaxFeatures = t
 return MovingObject.maxDistance(self, self, tMaxFeatures)
 else:
 print('Load features to compute a maximum size')
 return None
 def setRoutes(self, startRouteID, endRouteID):
 self.startRouteID = startRouteID
 self.endRouteID = endRouteID
 def getInstantsCrossingLane(self, p1, p2):
 '''Returns the instant(s)
 at which the object passes from one side of the segment to the other
 empty list if there is no crossing'''
 indices, intersections = self.positions.getIntersections(p1, p2)
 self.prototypeSimilarities = []
 for proto in prototypes:
 lcss.similarities(proto.getMovingObject().getPositions().asArray().T, self.getPositions().asArray().T)
 similarities = lcss.similarityTable[-1, :-1].astype(float)
 self.prototypeSimilarities.append(similarities/minimum(arange(1., len(similarities)+1), proto.getMovingObject().length()*ones(len(similarities))))
 @staticmethod
 def computePET(obj1, obj2, collisionDistanceThreshold):
 '''Post-encroachment time based on distance threshold
 Returns the smallest time difference when the object positions are within collisionDistanceThreshold
 ###
 def classifyUserTypeSpeedMotorized(self, threshold, aggregationFunc = median, nInstantsIgnoredAtEnds = 0):
 '''Classifies slow and fast road users
 slow: non-motorized -> pedestrians
 fast: motorized -> cars
 aggregationFunc can be any function that can be applied to a vector of speeds, including percentile:
 aggregationFunc = lambda x: percentile(x, percentileFactor) # where percentileFactor is 85 for 85th percentile'''
 speeds = self.getSpeeds(nInstantsIgnoredAtEnds)
 if aggregationFunc(speeds) >= threshold:
 self.setUserType(userType2Num['car'])
 return '{} {} {}'.format(self.filename, self.num, self.trajectoryType)
 def __eq__(self, p2):
 return self.filename == p2.filename and self.num == p2.num and self.trajectoryType == p2.trajectoryType
 def __hash__(self):
 return hash((self.filename, self.num, self.trajectoryType))
 ##################
 # Annotations
 ##################
 class BBMovingObject(MovingObject):
 print('{} '.format(t)+', '.join(['{} {}'.format(k.getNum(), v.getNum()) for k,v in matches.items()]))
 if returnMatches:
 for a,o in matches.items():
 gtMatches[a.getNum()][t] = o.getNum()
 toMatches[o.getNum()][t] = a.getNum()
 # compute metrics elements
 ct += len(matches)
 mt += nGTs-len(matches)
 fpt += nTOs-len(matches)
 gt += nGTs
 if debug:
 for mm in set(mismatches):
 print('{} {}'.format(type(mm), mm.getNum()))
 # some object mismatches may appear twice
 mme += len(set(mismatches))
 if ct > 0:
 motp = dist/ct
 else:
 motp = None
 if gt > 0:

Mercurial Hosting > traffic-intelligence

comparison trafficintelligence/moving.py @ 1110:6bbcd9433732