Mercurial Hosting > traffic-intelligence
comparison python/moving.py @ 939:a2f3f3ca241e
work in progress
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 17 Jul 2017 17:56:52 -0400 |
parents | b67a784beb69 |
children | b1e8453c207c |
comparison
equal
deleted
inserted
replaced
938:fbf12382f3f8 | 939:a2f3f3ca241e |
---|---|
219 if clockwise: | 219 if clockwise: |
220 return Point(self.y, -self.x) | 220 return Point(self.y, -self.x) |
221 else: | 221 else: |
222 return Point(-self.y, self.x) | 222 return Point(-self.y, self.x) |
223 | 223 |
224 def multiply(self, alpha): | 224 def __mul__(self, alpha): |
225 'Warning, returns a new Point' | 225 'Warning, returns a new Point' |
226 return Point(self.x*alpha, self.y*alpha) | 226 return Point(self.x*alpha, self.y*alpha) |
227 | 227 |
228 def divide(self, alpha): | 228 def divide(self, alpha): |
229 'Warning, returns a new Point' | 229 'Warning, returns a new Point' |
428 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y)) | 428 print('qx={0}, qy={1}, p0x={2}, p0y={3}, p1x={4}, p1y={5}...'.format(qx, qy, p0x, p0y, p1x, p1y)) |
429 import pdb; pdb.set_trace() | 429 import pdb; pdb.set_trace() |
430 return Point(X,Y) | 430 return Point(X,Y) |
431 | 431 |
432 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5): | 432 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5): |
433 ''' Snap a point p to it's nearest subsegment of it's nearest spline (from the list splines). | 433 ''' Snap a point p to its nearest subsegment of it's nearest spline (from the list splines). |
434 A spline is a list of points (class Point), most likely a trajectory. | 434 A spline is a list of points (class Point), most likely a trajectory. |
435 | 435 |
436 Output: | 436 Output: |
437 ======= | 437 ======= |
438 [spline index, | 438 [spline index, |
542 return Point(self.norm*cos(self.angle), self.norm*sin(self.angle)) | 542 return Point(self.norm*cos(self.angle), self.norm*sin(self.angle)) |
543 | 543 |
544 | 544 |
545 def predictPositionNoLimit(nTimeSteps, initialPosition, initialVelocity, initialAcceleration = Point(0,0)): | 545 def predictPositionNoLimit(nTimeSteps, initialPosition, initialVelocity, initialAcceleration = Point(0,0)): |
546 '''Predicts the position in nTimeSteps at constant speed/acceleration''' | 546 '''Predicts the position in nTimeSteps at constant speed/acceleration''' |
547 return initialVelocity + initialAcceleration.multiply(nTimeSteps),initialPosition+initialVelocity.multiply(nTimeSteps) + initialAcceleration.multiply(nTimeSteps**2*0.5) | 547 return initialVelocity + initialAcceleration.__mul__(nTimeSteps),initialPosition+initialVelocity.__mul__(nTimeSteps) + initialAcceleration.__mul__(nTimeSteps**2*0.5) |
548 | 548 |
549 def predictPosition(position, speedOrientation, control, maxSpeed = None): | 549 def predictPosition(position, speedOrientation, control, maxSpeed = None): |
550 '''Predicts the position (moving.Point) at the next time step with given control input (deltaSpeed, deltaTheta) | 550 '''Predicts the position (moving.Point) at the next time step with given control input (deltaSpeed, deltaTheta) |
551 speedOrientation is the other encoding of velocity, (speed, orientation) | 551 speedOrientation is the other encoding of velocity, (speed, orientation) |
552 speedOrientation and control are NormAngle''' | 552 speedOrientation and control are NormAngle''' |
566 self.velocity = velocity | 566 self.velocity = velocity |
567 | 567 |
568 def __add__(self, other): | 568 def __add__(self, other): |
569 return FlowVector(self.position+other.position, self.velocity+other.velocity) | 569 return FlowVector(self.position+other.position, self.velocity+other.velocity) |
570 | 570 |
571 def multiply(self, alpha): | 571 def __mul__(self, alpha): |
572 return FlowVector(self.position.multiply(alpha), self.velocity.multiply(alpha)) | 572 return FlowVector(self.position.__mul__(alpha), self.velocity.__mul__(alpha)) |
573 | 573 |
574 def plot(self, options = '', **kwargs): | 574 def plot(self, options = '', **kwargs): |
575 plot([self.position.x, self.position.x+self.velocity.x], [self.position.y, self.position.y+self.velocity.y], options, **kwargs) | 575 plot([self.position.x, self.position.x+self.velocity.x], [self.position.y, self.position.y+self.velocity.y], options, **kwargs) |
576 self.position.plot(options+'x', **kwargs) | 576 self.position.plot(options+'x', **kwargs) |
577 | 577 |
588 det = float(dp34.y*dp12.x-dp34.x*dp12.y) | 588 det = float(dp34.y*dp12.x-dp34.x*dp12.y) |
589 if det == 0.: | 589 if det == 0.: |
590 return None | 590 return None |
591 else: | 591 else: |
592 ua = (dp34.x*(p1.y-p3.y)-dp34.y*(p1.x-p3.x))/det | 592 ua = (dp34.x*(p1.y-p3.y)-dp34.y*(p1.x-p3.x))/det |
593 return p1+dp12.multiply(ua) | 593 return p1+dp12.__mul__(ua) |
594 | 594 |
595 # def intersection(p1, p2, dp1, dp2): | 595 # def intersection(p1, p2, dp1, dp2): |
596 # '''Returns the intersection point between the two lines | 596 # '''Returns the intersection point between the two lines |
597 # defined by the respective vectors (dp) and origin points (p)''' | 597 # defined by the respective vectors (dp) and origin points (p)''' |
598 # from numpy import matrix | 598 # from numpy import matrix |
793 return None | 793 return None |
794 else: | 794 else: |
795 return Trajectory([[a-b for a,b in zip(self.getXCoordinates(),traj2.getXCoordinates())], | 795 return Trajectory([[a-b for a,b in zip(self.getXCoordinates(),traj2.getXCoordinates())], |
796 [a-b for a,b in zip(self.getYCoordinates(),traj2.getYCoordinates())]]) | 796 [a-b for a,b in zip(self.getYCoordinates(),traj2.getYCoordinates())]]) |
797 | 797 |
798 def multiply(self, alpha): | 798 def __mul__(self, alpha): |
799 '''Returns a new trajectory of the same length''' | 799 '''Returns a new trajectory of the same length''' |
800 return Trajectory([[alpha*x for x in self.getXCoordinates()], | 800 return Trajectory([[alpha*x for x in self.getXCoordinates()], |
801 [alpha*y for y in self.getYCoordinates()]]) | 801 [alpha*y for y in self.getYCoordinates()]]) |
802 | 802 |
803 def differentiate(self, doubleLastPosition = False): | 803 def differentiate(self, doubleLastPosition = False): |
874 i = -1 | 874 i = -1 |
875 for p2 in self: | 875 for p2 in self: |
876 distances2.append(Point.distanceNorm2(p1, p2)) | 876 distances2.append(Point.distanceNorm2(p1, p2)) |
877 if distances2[-1] < minDist2: | 877 if distances2[-1] < minDist2: |
878 minDist2 = distances2[-1] | 878 minDist2 = distances2[-1] |
879 i = len(distances)-1 | 879 i = len(distances2)-1 |
880 if maxDist2 is None or (maxDist2 is not None and minDist2 < maxDist2): | 880 if maxDist2 is not None and minDist2 < maxDist2: |
881 return None | |
882 else: | |
881 return i | 883 return i |
882 else: | |
883 return None | |
884 | 884 |
885 def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5): | 885 def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5): |
886 '''Indicates whether the minProportion (<=1.) (eg half) of the trajectory elements (vectors for velocity) | 886 '''Indicates whether the minProportion (<=1.) (eg half) of the trajectory elements (vectors for velocity) |
887 have a cosine with refDirection is smaller than cosineThreshold''' | 887 have a cosine with refDirection is smaller than cosineThreshold''' |
888 count = 0 | 888 count = 0 |
1138 if obj.existsAtInstant(t): | 1138 if obj.existsAtInstant(t): |
1139 if obj.hasFeatures(): | 1139 if obj.hasFeatures(): |
1140 n = len([f for f in obj.getFeatures() if f.existsAtInstant(t)]) | 1140 n = len([f for f in obj.getFeatures() if f.existsAtInstant(t)]) |
1141 else: | 1141 else: |
1142 n = 1. | 1142 n = 1. |
1143 p += obj.getPositionAtInstant(t).multiply(n) | 1143 p += obj.getPositionAtInstant(t).__mul__(n) |
1144 nTotal += n | 1144 nTotal += n |
1145 assert nTotal>0, 'there should be at least one point for each instant' | 1145 assert nTotal>0, 'there should be at least one point for each instant' |
1146 positions.addPosition(p.divide(nTotal)) | 1146 positions.addPosition(p.divide(nTotal)) |
1147 # velocities: if any | 1147 # velocities: if any |
1148 if hasattr(obj1, 'velocities') and hasattr(obj2, 'velocities'): | 1148 if hasattr(obj1, 'velocities') and hasattr(obj2, 'velocities'): |
1154 if obj.existsAtInstant(t): | 1154 if obj.existsAtInstant(t): |
1155 if obj.hasFeatures(): | 1155 if obj.hasFeatures(): |
1156 n = len([f for f in obj.getFeatures() if f.existsAtInstant(t)]) | 1156 n = len([f for f in obj.getFeatures() if f.existsAtInstant(t)]) |
1157 else: | 1157 else: |
1158 n = 1. | 1158 n = 1. |
1159 p += obj.getVelocityAtInstant(t).multiply(n) | 1159 p += obj.getVelocityAtInstant(t).__mul__(n) |
1160 nTotal += n | 1160 nTotal += n |
1161 assert n>0, 'there should be at least one point for each instant' | 1161 assert n>0, 'there should be at least one point for each instant' |
1162 velocities.addPosition(p.divide(nTotal)) | 1162 velocities.addPosition(p.divide(nTotal)) |
1163 else: | 1163 else: |
1164 velocities = None | 1164 velocities = None |
1729 super(BBMovingObject, self).__init__(num, timeInterval, userType = userType) | 1729 super(BBMovingObject, self).__init__(num, timeInterval, userType = userType) |
1730 self.topLeftPositions = topLeftPositions.getPositions() | 1730 self.topLeftPositions = topLeftPositions.getPositions() |
1731 self.bottomRightPositions = bottomRightPositions.getPositions() | 1731 self.bottomRightPositions = bottomRightPositions.getPositions() |
1732 | 1732 |
1733 def computeCentroidTrajectory(self, homography = None): | 1733 def computeCentroidTrajectory(self, homography = None): |
1734 self.positions = self.topLeftPositions.add(self.bottomRightPositions).multiply(0.5) | 1734 self.positions = self.topLeftPositions.add(self.bottomRightPositions).__mul__(0.5) |
1735 if homography is not None: | 1735 if homography is not None: |
1736 self.positions = self.positions.homographyProject(homography) | 1736 self.positions = self.positions.homographyProject(homography) |
1737 | 1737 |
1738 def matches(self, obj, instant, matchingDistance): | 1738 def matches(self, obj, instant, matchingDistance): |
1739 '''Indicates if the annotation matches obj (MovingObject) | 1739 '''Indicates if the annotation matches obj (MovingObject) |