Mercurial > hg > nsaunier > traffic-intelligence

comparison trafficintelligence/moving.py @ 1210:1bad5f9b60de

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
work in progress
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Fri, 28 Apr 2023 17:03:39 -0400
parents 2064e52019db
children af329f3330ba
comparison
equal deleted inserted replaced
1209:2064e52019db 1210:1bad5f9b60de
3 3
4 import copy 4 import copy
5 from math import sqrt, atan2, cos, sin 5 from math import sqrt, atan2, cos, sin
6 6
7 from numpy import median, mean, array, arange, zeros, ones, hypot, NaN, std, floor, ceil, float32, argwhere, minimum, issubdtype, integer as npinteger 7 from numpy import median, mean, array, arange, zeros, ones, hypot, NaN, std, floor, ceil, float32, argwhere, minimum, issubdtype, integer as npinteger
8 from matplotlib.pyplot import plot, text 8 from matplotlib.pyplot import plot, text, arrow
9 from scipy.stats import scoreatpercentile 9 from scipy.stats import scoreatpercentile
10 from scipy.spatial.distance import cdist 10 from scipy.spatial.distance import cdist
11 from scipy.signal import savgol_filter 11 from scipy.signal import savgol_filter
12 12
13 try: 13 try:
428 originalE1 = Point(Point.dot(e1, Point(1,0)),Point.dot(e2, Point(1,0))) 428 originalE1 = Point(Point.dot(e1, Point(1,0)),Point.dot(e2, Point(1,0)))
429 originalE2 = Point(Point.dot(e1, Point(0,1)),Point.dot(e2, Point(0,1))) 429 originalE2 = Point(Point.dot(e1, Point(0,1)),Point.dot(e2, Point(0,1)))
430 return [Point(Point.dot(originalE1, p), Point.dot(originalE2, p)) for p in [frontLeft, frontRight, rearRight, rearLeft]] 430 return [Point(Point.dot(originalE1, p), Point.dot(originalE2, p)) for p in [frontLeft, frontRight, rearRight, rearLeft]]
431 431
432 if shapelyAvailable: 432 if shapelyAvailable:
433 def pointsToShapely(points):
434 'Returns shapely polygon'
435 return Polygon([p.astuple() for p in points])
436
433 def pointsInPolygon(points, polygon): 437 def pointsInPolygon(points, polygon):
434 '''Optimized tests of a series of points within (Shapely) polygon (not prepared)''' 438 '''Optimized tests of a series of points within (Shapely) polygon (not prepared)'''
435 if type(polygon) == PreparedGeometry: 439 if type(polygon) == PreparedGeometry:
436 prepared_polygon = polygon 440 prepared_polygon = polygon
437 else: 441 else:
1230 q2=self.__getitem__(i+1) 1234 q2=self.__getitem__(i+1)
1231 if q1[0] <= S1 < q2[0] and (lane is None or (self.lanes[i] == lane and self.lanes[i+1] == lane)): 1235 if q1[0] <= S1 < q2[0] and (lane is None or (self.lanes[i] == lane and self.lanes[i+1] == lane)):
1232 indices.append(i+(S1-q1[0])/(q2[0]-q1[0])) 1236 indices.append(i+(S1-q1[0])/(q2[0]-q1[0]))
1233 return indices 1237 return indices
1234 1238
1239 ##################
1240 # Geometry
1241 ##################
1242
1243 class Geometry(object):
1244 '''Generic class for outline and size of object '''
1245
1246
1247 class CarGeometry(Geometry):
1248 '''Default car geometry as rectangle '''
1249
1235 ################## 1250 ##################
1236 # Moving Objects 1251 # Moving Objects
1237 ################## 1252 ##################
1238 1253
1239 userTypeNames = ['unknown', 1254 userTypeNames = ['unknown',
1654 if withIds: 1669 if withIds:
1655 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, self.getNum(), **kwargs) 1670 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, self.getNum(), **kwargs)
1656 else: 1671 else:
1657 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, None, **kwargs) 1672 self.positions.plotOnWorldImage(nPixelsPerUnitDistance, options, withOrigin, timeStep, None, **kwargs)
1658 1673
1659 def plotOutlineAtInstant(self, t, options = '', **kwargs): 1674 def plotOutlineAtInstant(self, t, options = '', withVelocity = False, velocityMultiply = 5, arrowWidth=0.1, **kwargs):
1660 if self.hasFeatures(): 1675 if self.hasFeatures():
1661 points = [f.getPositionAtInstant(t) for f in self.getFeatures()] 1676 points = [f.getPositionAtInstant(t) for f in self.getFeatures()]
1662 Point.plotAll(points, True, options, **kwargs) 1677 Point.plotAll(points, True, options, **kwargs)
1678 if withVelocity:
1679 p = self.getPositionAtInstant(t)
1680 v = self.getVelocityAtInstant(t)*velocityMultiply
1681 arrow(p.x, p.y, v.x, v.y, width=arrowWidth)
1663 1682
1664 def play(self, videoFilename, homography = None, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1.): 1683 def play(self, videoFilename, homography = None, undistort = False, intrinsicCameraMatrix = None, distortionCoefficients = None, undistortedImageMultiplication = 1.):
1665 cvutils.displayTrajectories(videoFilename, [self], homography = homography, firstFrameNum = self.getFirstInstant(), lastFrameNumArg = self.getLastInstant(), undistort = undistort, intrinsicCameraMatrix = intrinsicCameraMatrix, distortionCoefficients = distortionCoefficients, undistortedImageMultiplication = undistortedImageMultiplication) 1684 cvutils.displayTrajectories(videoFilename, [self], homography = homography, firstFrameNum = self.getFirstInstant(), lastFrameNumArg = self.getLastInstant(), undistort = undistort, intrinsicCameraMatrix = intrinsicCameraMatrix, distortionCoefficients = distortionCoefficients, undistortedImageMultiplication = undistortedImageMultiplication)
1666 1685
1667 def speedDiagnostics(self, framerate = 1., display = False, nInstantsIgnoredAtEnds=0): 1686 def speedDiagnostics(self, framerate = 1., display = False, nInstantsIgnoredAtEnds=0):
1807 xj = array(fj.getXCoordinates()[inter.first-fj.getFirstInstant():int(fj.length())-(fj.getLastInstant()-inter.last)]) 1826 xj = array(fj.getXCoordinates()[inter.first-fj.getFirstInstant():int(fj.length())-(fj.getLastInstant()-inter.last)])
1808 yj = array(fj.getYCoordinates()[inter.first-fj.getFirstInstant():int(fj.length())-(fj.getLastInstant()-inter.last)]) 1827 yj = array(fj.getYCoordinates()[inter.first-fj.getFirstInstant():int(fj.length())-(fj.getLastInstant()-inter.last)])
1809 relativePositions[(i,j)] = Point(median(xj-xi), median(yj-yi)) 1828 relativePositions[(i,j)] = Point(median(xj-xi), median(yj-yi))
1810 relativePositions[(j,i)] = -relativePositions[(i,j)] 1829 relativePositions[(j,i)] = -relativePositions[(i,j)]
1811 1830
1812 def computeBoundingPolygon(self, instant): 1831 def computeBoundingPolygon(self, t, shape = 'raw'):
1813 '''Returns a bounding box for the feature positions at instant 1832 '''Returns a bounding box for the feature positions at instant
1814 bounding box format is a list of points (4 in this case for a rectangle) 1833 bounding box format is a list of points
1815 1834 shape is the type of output shape:
1816 TODO add method argument if using different methods/shapes''' 1835 - raw means the list of feature positions
1836 - rect means the bounding rectangle aligned with velocity'''
1817 if self.hasFeatures(): 1837 if self.hasFeatures():
1818 positions = [f.getPositionAtInstant(instant) for f in self.getFeatures() if f.existsAtInstant(instant)] 1838 positions = [f.getPositionAtInstant(t) for f in self.getFeatures() if f.existsAtInstant(t)]
1819 return Point.boundingRectangle(positions, self.getVelocityAtInstant(instant)) 1839 if shape == 'rect':
1840 return Point.boundingRectangle(positions, self.getVelocityAtInstant(t))
1841 elif shape == 'raw':
1842 return positions
1843 else:
1844 print('Unknown shape')
1845 return None
1820 else: 1846 else:
1821 print('Object {} has no features'.format(self.getNum())) 1847 print('Object {} has no features'.format(self.getNum()))
1822 return None 1848 return None
1823 1849
1824 def motDistanceAtInstant(self, obj, instant): 1850 def motDistanceAtInstant(self, obj, instant):