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

comparison trafficintelligence/moving.py @ 1096:9a32d63bae3f

update from Lionel Nebot Janvier

author	Nicolas Saunier <nicolas.saunier@polymtl.ca>
date	Fri, 15 Feb 2019 14:35:56 -0500
parents	c96388c696ac
children	b3f8b26ee838

comparison

equal deleted inserted replaced

-:e53c6e87bb3f
+:9a32d63bae3f
 #     with methods to create intersection, unions...
 #     '''
 # We will use the polygon class of Shapely
 class STObject(object):
 '''Class for spatio-temporal object, i.e. with temporal and spatial existence
 (time interval and bounding polygon for positions (e.g. rectangle)).
 It may not mean that the object is defined
 for all time instants within the time interval'''
 def __init__(self, num = None, timeInterval = None, boundingPolygon = None):
 self.num = num
 self.timeInterval = timeInterval
 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, 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.
 Output:
 =======
 [alignment index,
 subsegment leading point index,
 snapped point,
 closestPoint = ppldb2p(p.x,p.y,alignments[alignmentIdx][alignment_p][0],alignments[alignmentIdx][alignment_p][1],alignments[alignmentIdx][alignment_p+1][0],alignments[alignmentIdx][alignment_p+1][1])
 if closestPoint is None:
 print('Error: Alignment {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(alignmentIdx, alignment_p))
 return None
 # check if the projected point is in between the current segment of the alignment bounds
 if utils.inBetween(alignments[alignmentIdx][alignment_p][0], alignments[alignmentIdx][alignment_p+1][0], closestPoint.x) and utils.inBetween(alignments[alignmentIdx][alignment_p][1], alignments[alignmentIdx][alignment_p+1][1], closestPoint.y):
 offsetY = Point.distanceNorm2(closestPoint, p)
 if offsetY < minOffsetY:
 minOffsetY = offsetY
 snappedAlignmentIdx = alignmentIdx
 snappedAlignmentLeadingPoint = alignment_p
 snappedPoint = Point(closestPoint.x, closestPoint.y)
 #Jump loop if significantly close
 if offsetY < goodEnoughAlignmentDistance:
 break
 #Get sub-segment distance
 if minOffsetY != float('inf'):
 subsegmentDistance = Point.distanceNorm2(snappedPoint, alignments[snappedAlignmentIdx][snappedAlignmentLeadingPoint])
 else:
 print('Offset for point {} is infinite (check with prepareAlignments if some alignment segments are aligned with axes)'.format(p))
 return None
 def getXYfromSY(s, y, alignmentNum, alignments):
 ''' Find X,Y coordinate from S,Y data.
 if mode = 0 : return Snapped X,Y
 if mode !=0 : return Real X,Y
 '''
 alignment = alignments[alignmentNum]
 i = 1
 while s > alignment.getCumulativeDistance(i) and i < len(alignment):
 i += 1
 if i < len(alignment):
 else:
 ua = (dp34.x*(p1.y-p3.y)-dp34.y*(p1.x-p3.x))/det
 return p1+dp12.__mul__(ua)
 # def intersection(p1, p2, dp1, dp2):
 #     '''Returns the intersection point between the two lines
 #     defined by the respective vectors (dp) and origin points (p)'''
 #     from numpy import matrix
 #     from numpy.linalg import linalg
 #     A = matrix([[dp1.y, -dp1.x],
 #                 [dp2.y, -dp2.x]])
 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()):
 return None
 else:
 inter = intersection(p1, p2, p3, p4)
 if (inter is not None
 and utils.inBetween(p1.x, p2.x, inter.x)
 and utils.inBetween(p3.x, p4.x, inter.x)
 and utils.inBetween(p1.y, p2.y, inter.y)
 and utils.inBetween(p3.y, p4.y, inter.y)):
 return inter
 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 getClosestPoint(self, p1, maxDist2 = None):
 '''Returns the instant of the closest position in trajectory to p1 (and the point)
 return None
 else:
 return i
 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
 for p in self:
 if p.similarOrientation(refDirection, cosineThreshold):
 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
 Returns an empty list if there is no crossing'''
 indices = []
 intersections = []
 for i in range(self.length()-1):
 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 range(self.length()-1):
 def getTrajectoryInPolygon(self, polygon, t2 = None):
 '''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()
 inPolygon = []
 for x, y in zip(self.positions[0], self.positions[1]):
 inPolygon.append(polygon.contains(shapelyPoint(x, y)))
 class CurvilinearTrajectory(Trajectory):
 '''Sub class of trajectory for trajectories with curvilinear coordinates and lane assignements
 longitudinal coordinate is stored as first coordinate (exterior name S)
 lateral coordinate is stored as second coordinate
 the third "lane" coordinate is for an alignment id,
 whether explicit for a list/dict of alignments,
 or implicit for a road with lane numbers'''
 def __init__(self, S = None, Y = None, lanes = None):
 if S is None or Y is None or len(S) != len(Y):
 self.positions = [[],[]]
 print('Warning: trajectory at point {} {} has alignment errors (alignment snapping)\nCurvilinear trajectory could not be computed'.format(i, t[i]))
 else:
 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
 curvilinearPositions.addPositionSYL(S, Y, align)
 ## Go back through points and correct lane
 #Run through objects looking for outlier point
 smoothed_lanes = utils.filterCategoricalMovingWindow(curvilinearPositions.getLanes(), halfWidth)
 ## Recalculate projected point to new lane
 lanes = curvilinearPositions.getLanes()
 if(lanes != smoothed_lanes):
 else:
 [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
 curvilinearPositions.setPosition(i, S, Y, align)
 return curvilinearPositions
 def __getitem__(self,i):
 if isinstance(i, int):
 return [self.positions[0][i], self.positions[1][i], self.lanes[i]]
 else:
 raise TypeError("Invalid argument type.")
 #elif isinstance( key, slice ):
 if doubleLastPosition and self.length() > 1:
 diff.addPosition(diff[-1])
 return diff
 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)
 Returns an empty list if there is no crossing'''
 indices = []
 for i in range(self.length()-1):
 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)
 '''
 def __init__(self, num = None, timeInterval = None, positions = None, velocities = None, geometry = None, userType = userType2Num['unknown'], nObjects = None):
 super(MovingObject, self).__init__(num, timeInterval)
 def updatePositions(self):
 inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval())
 def updateCurvilinearPositions(self, method, changeOfAlignment, nextAlignment_idx, timeStep = None, time = None,
-leaderVehicleCurvilinearPositionAtPrecedentTime = None, previousAlignmentId = None,
+leaderVehicle = None, previousAlignmentId = None,
-maxSpeed = None, acceleration = None, seed = None, delta = None):
+maxSpeed = None, acceleration = None):
-import math
 if method == 'newell':
-self.curvilinearPositions.addPositionSYL(leaderVehicleCurvilinearPositionAtPrecedentTime - self.dn, 0, nextAlignment_idx)
+if time <= self.timeInterval[0] < time + timeStep:
-if changeOfAlignment:
+# #si t < instant de creation du vehicule, la position vaut l'espacement dn entre les deux vehicules
-self.velocities.addPositionSYL((self.curvilinearPositions[-1][0]-self.curvilinearPositions[-2][0])/timeStep,
+if leaderVehicle is None:
-(self.curvilinearPositions[-1][1]-self.curvilinearPositions[-1][1])/timeStep,
+self.curvilinearPositions.addPositionSYL(
-(previousAlignmentId, nextAlignment_idx))
+-self.dn,
+0,
+nextAlignment_idx)
+else:
+self.curvilinearPositions.addPositionSYL(
+leaderVehicle.curvilinearPositions[0][0] - self.dn,
+0,
+nextAlignment_idx)
 else:
-self.velocities.addPositionSYL((self.curvilinearPositions[-1][0]-self.curvilinearPositions[-2][0])/timeStep,
+if leaderVehicle is None:
-(self.curvilinearPositions[-1][1]-self.curvilinearPositions[-1][1])/timeStep,
+self.curvilinearPositions.addPositionSYL(self.curvilinearPositions[-1][0]+self.desiredSpeed*timeStep, 0, nextAlignment_idx)
-None)
+else:
-elif method == 'constantSpeed':
+if time > self.reactionTime:
-random.seed(seed)
+previousVehicleCurvilinearPositionAtPrecedentTime = \
-self.curvilinearPositions.addPositionSYL(self.curvilinearPositions[time-1][0] + self.desiredSpeed * timeStep,
+leaderVehicle.curvilinearPositions[-int(round(self.reactionTime))][0]  # t-v.reactionTime
-0,
+else:
-nextAlignment_idx)
+previousVehicleCurvilinearPositionAtPrecedentTime = \
+leaderVehicle.curvilinearPositions[0][0]
+self.curvilinearPositions.addPositionSYL(previousVehicleCurvilinearPositionAtPrecedentTime - self.dn, 0, nextAlignment_idx)
+#mise ajour des vitesses
+if changeOfAlignment:
+self.velocities.addPositionSYL((self.curvilinearPositions[-1][0]-self.curvilinearPositions[-2][0])/timeStep,
+(self.curvilinearPositions[-1][1]-self.curvilinearPositions[-2][1])/timeStep,
+(previousAlignmentId, nextAlignment_idx))
+else:
+self.velocities.addPositionSYL((self.curvilinearPositions[-1][0]-self.curvilinearPositions[-2][0])/timeStep,
+(self.curvilinearPositions[-1][1]-self.curvilinearPositions[-2][1])/timeStep,
+None)
 @staticmethod
 def concatenate(obj1, obj2, num = None, newFeatureNum = None, computePositions = False):
 '''Concatenates two objects, whether overlapping temporally or not
 Positions will be recomputed only if computePositions is True
 def getNLongestFeatures(self, nFeatures = 1):
 if self.features is None:
 return []
 else:
 tmp = utils.sortByLength(self.getFeatures(), reverse = True)
 return tmp[:min(len(tmp), nFeatures)]
 def getFeatureNumbersOverTime(self):
 '''Returns the number of features at each instant
 dict instant -> number of features'''
 if self.hasFeatures():
 featureNumbers = {}
 else:
 return int(commonTimeInterval.length()), None, None
 @staticmethod
 def distances(obj1, obj2, instant1, _instant2 = None):
 '''Returns the distances between all features of the 2 objects
 at the same instant instant1
 or at instant1 and instant2'''
 if _instant2 is None:
 instant2 = instant1
 else:
 return smallPets[petIdx], obj1.getFirstInstant()+distanceIndices[0], obj2.getFirstInstant()+distanceIndices[1]
 else:
 return None, None, None
 def predictPosition(self, instant, nTimeSteps, externalAcceleration = Point(0,0)):
 '''Predicts the position of object at instant+deltaT,
 at constant speed'''
 return predictPositionNoLimit(nTimeSteps, self.getPositionAtInstant(instant), self.getVelocityAtInstant(instant), externalAcceleration)
 def projectCurvilinear(self, alignments, halfWidth = 3):
 self.curvilinearPositions = CurvilinearTrajectory.fromTrajectoryProjection(self.getPositions(), alignments, halfWidth)
 def computeSmoothTrajectory(self, minCommonIntervalLength):
 '''Computes the trajectory as the mean of all features
 if a feature exists, its position is
 Warning work in progress
 TODO? not use the first/last 1-.. positions'''
 nFeatures = len(self.features)
 if nFeatures == 0:
 print('Empty object features\nCannot compute smooth trajectory')
 f.positions = Trajectory(pp)
 self.userTypes = {}
 def classifyUserTypeHoGSVMAtInstant(self, img, instant, width, height, px, py, minNPixels, rescaleSize, orientations, pixelsPerCell, cellsPerBlock, blockNorm):
 '''Extracts the image box around the object
 (of square size max(width, height) of the box around the features,
 with an added px or py for width and height (around the box))
 computes HOG on this cropped image (with parameters rescaleSize, orientations, pixelsPerCell, cellsPerBlock)
 and applies the SVM model on it'''
 croppedImg = cvutils.imageBox(img, self, instant, width, height, px, py, minNPixels)
 if croppedImg is not None and len(croppedImg) > 0:
 '''Agregates SVM detections in each image and returns probability
 (proportion of instants with classification in each category)
 images is a dictionary of images indexed by instant
 With default parameters, the general (ped-bike-car) classifier will be used
 Considered categories are the keys of speedProbabilities'''
 if not hasattr(self, 'aggregatedSpeed') or not hasattr(self, 'userTypes'):
 print('Initializing the data structures for classification by HoG-SVM')
 self.initClassifyUserTypeHoGSVM(aggregationFunc, pedBikeCarSVM, bikeCarSVM, pedBikeSpeedTreshold, bikeCarSpeedThreshold, nInstantsIgnoredAtEnds)
 else:
 self.setUserType(utils.argmaxDict(userTypeProbabilities))
 def classifyUserTypeArea(self, areas, homography):
 '''Classifies the object based on its location (projected to image space)
 areas is a dictionary of matrix of the size of the image space
 for different road users possible locations, indexed by road user type names
 TODO: areas could be a wrapper object with a contains method that would work for polygons and images (with wrapper class)
 skip frames at beginning/end?'''
 print('not implemented/tested yet')
 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(BBMovingObject, self).__init__(num, timeInterval, userType = userType)
 gt number of GT.frames
 if returnMatches is True, return as 2 new arguments the GT and TO matches
 matches is a dict
 matches[i] is the list of matches for GT/TO i
 the list of matches is a dict, indexed by time, for the TO/GT id matched at time t
 (an instant t not present in matches[i] at which GT/TO exists means a missed detection or false alarm)
 TODO: Should we use the distance as weights or just 1/0 if distance below matchingDistance?
 (add argument useDistanceForWeights = False)'''
 from munkres import Munkres
 munk = Munkres()
 dist = 0. # total distance between GT and TO
 ct = 0 # number of associations between GT and tracker output in each frame
 gt = 0 # number of GT.frames
 mt = 0 # number of missed GT.frames (sum of the number of GT not detected in each frame)
 elif matches[a] in list(previousMatches.values()):
 mismatches.append(matches[a])
 for a in previousMatches:
 if a not in matches and previousMatches[a] in list(matches.values()):
 mismatches.append(previousMatches[a])
 if debug:
 for mm in set(mismatches):
 print('{} {}'.format(type(mm), mm.getNum()))
 # some object mismatches may appear twice
 mme += len(set(mismatches))
 suite = doctest.DocFileSuite('tests/moving.txt')
 #suite = doctest.DocTestSuite()
 unittest.TextTestRunner().run(suite)
 #doctest.testmod()
 #doctest.testfile("example.txt")
 if shapelyAvailable:
 suite = doctest.DocFileSuite('tests/moving_shapely.txt')
 unittest.TextTestRunner().run(suite)

Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/moving.py @ 1096:9a32d63bae3f