Mercurial Hosting > traffic-intelligence
changeset 1210:1bad5f9b60de
work in progress
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Fri, 28 Apr 2023 17:03:39 -0400 |
parents | 2064e52019db |
children | a095d4fbb2ea |
files | trafficintelligence/moving.py trafficintelligence/prediction.py |
diffstat | 2 files changed, 51 insertions(+), 8 deletions(-) [+] |
line wrap: on
line diff
--- a/trafficintelligence/moving.py Wed Apr 26 23:36:32 2023 -0400 +++ b/trafficintelligence/moving.py Fri Apr 28 17:03:39 2023 -0400 @@ -5,7 +5,7 @@ from math import sqrt, atan2, cos, sin from numpy import median, mean, array, arange, zeros, ones, hypot, NaN, std, floor, ceil, float32, argwhere, minimum, issubdtype, integer as npinteger -from matplotlib.pyplot import plot, text +from matplotlib.pyplot import plot, text, arrow from scipy.stats import scoreatpercentile from scipy.spatial.distance import cdist from scipy.signal import savgol_filter @@ -430,6 +430,10 @@ return [Point(Point.dot(originalE1, p), Point.dot(originalE2, p)) for p in [frontLeft, frontRight, rearRight, rearLeft]] if shapelyAvailable: + def pointsToShapely(points): + 'Returns shapely polygon' + return Polygon([p.astuple() for p in points]) + def pointsInPolygon(points, polygon): '''Optimized tests of a series of points within (Shapely) polygon (not prepared)''' if type(polygon) == PreparedGeometry: @@ -1233,6 +1237,17 @@ return indices ################## +# Geometry +################## + +class Geometry(object): + '''Generic class for outline and size of object ''' + + +class CarGeometry(Geometry): + '''Default car geometry as rectangle ''' + +################## # Moving Objects ################## @@ -1656,10 +1671,14 @@ else: self.positions.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, None, **kwargs) - def plotOutlineAtInstant(self, t, options = '', **kwargs): + def plotOutlineAtInstant(self, t, options = '', withVelocity = False, velocityMultiply = 5, arrowWidth=0.1, **kwargs): if self.hasFeatures(): points = [f.getPositionAtInstant(t) for f in self.getFeatures()] Point.plotAll(points, True, options, **kwargs) + if withVelocity: + p = self.getPositionAtInstant(t) + v = self.getVelocityAtInstant(t)*velocityMultiply + arrow(p.x, p.y, v.x, v.y, width=arrowWidth) def play(self, videoFilename, homography = None, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1.): cvutils.displayTrajectories(videoFilename, [self], homography = homography, firstFrameNum = self.getFirstInstant(), lastFrameNumArg = self.getLastInstant(), undistort = undistort, intrinsicCameraMatrix = intrinsicCameraMatrix, distortionCoefficients = distortionCoefficients, undistortedImageMultiplication = undistortedImageMultiplication) @@ -1809,14 +1828,21 @@ relativePositions[(i,j)] = Point(median(xj-xi), median(yj-yi)) relativePositions[(j,i)] = -relativePositions[(i,j)] - def computeBoundingPolygon(self, instant): + def computeBoundingPolygon(self, t, shape = 'raw'): '''Returns a bounding box for the feature positions at instant - bounding box format is a list of points (4 in this case for a rectangle) - - TODO add method argument if using different methods/shapes''' + bounding box format is a list of points + shape is the type of output shape: + - raw means the list of feature positions + - rect means the bounding rectangle aligned with velocity''' if self.hasFeatures(): - positions = [f.getPositionAtInstant(instant) for f in self.getFeatures() if f.existsAtInstant(instant)] - return Point.boundingRectangle(positions, self.getVelocityAtInstant(instant)) + positions = [f.getPositionAtInstant(t) for f in self.getFeatures() if f.existsAtInstant(t)] + if shape == 'rect': + return Point.boundingRectangle(positions, self.getVelocityAtInstant(t)) + elif shape == 'raw': + return positions + else: + print('Unknown shape') + return None else: print('Object {} has no features'.format(self.getNum())) return None
--- a/trafficintelligence/prediction.py Wed Apr 26 23:36:32 2023 -0400 +++ b/trafficintelligence/prediction.py Fri Apr 28 17:03:39 2023 -0400 @@ -170,6 +170,23 @@ collision = (p1-p2).norm2() <= collisionDistanceThreshold return collision, t, p1, p2 +def computeCollisionTimePolygon(predictedTrajectories1, predictedTrajectories2, timeHorizon): + '''Computes the first instant + at which two objects represented by a series of points (eg box) + Computes all the times including timeHorizon + + User has to check the first variable collision to know about a collision''' + t = 1 + poly1 = moving.pointsToShapely([t1.predictPosition(t) for t1 in predictedTrajectories1]) + poly2 = moving.pointsToShapely([t2.predictPosition(t) for t2 in predictedTrajectories2]) + collision = poly1.overlaps(poly2) + while t < timeHorizon and not collision: + t += 1 + poly1 = moving.pointsToShapely([t1.predictPosition(t) for t1 in predictedTrajectories1]) + poly2 = moving.pointsToShapely([t2.predictPosition(t) for t2 in predictedTrajectories2]) + collision = poly1.overlaps(poly2) + return collision, t, p1, p2 + def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon, printFigure = True): from matplotlib.pyplot import figure, axis, title, clf, savefig if printFigure: