Mercurial Hosting > traffic-intelligence
comparison trafficintelligence/moving.py @ 1210:1bad5f9b60de
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): |