Mercurial Hosting > traffic-intelligence
changeset 243:e0988a8ace0c
started adapting and moving to other modules Mohamed's work
author | Nicolas Saunier <nicolas.saunier@polymtl.ca> |
---|---|
date | Mon, 16 Jul 2012 04:57:35 -0400 |
parents | 942455aff829 |
children | 5027c174ab90 |
files | python/cvutils.py python/extrapolation.py python/moving.py |
diffstat | 3 files changed, 173 insertions(+), 175 deletions(-) [+] |
line wrap: on
line diff
--- a/python/cvutils.py Fri Jul 13 17:30:25 2012 -0400 +++ b/python/cvutils.py Mon Jul 16 04:57:35 2012 -0400 @@ -181,8 +181,8 @@ def project(homography, p): '''Returns the coordinates of the projection of the point p through homography''' - from numpy.core.multiarray import array - return projectArray(homography, array([[p[0]],p[1]])) + from numpy import array + return projectArray(homography, array([[p[0]],[p[1]]])) def projectTrajectory(homography, trajectory): '''Projects a series of points in the format
--- a/python/extrapolation.py Fri Jul 13 17:30:25 2012 -0400 +++ b/python/extrapolation.py Mon Jul 16 04:57:35 2012 -0400 @@ -1,173 +1,166 @@ -########## -# Extrapolation Hypothesis -########## - -import sys - -sys.path.append("G:/0-phdstart/Code/traffic-intelligence1/python") - -import moving - -#Default values -FPS= 25 # No. of frame per second (FPS) -vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec -deltaT= FPS*5 # extrapolatation time Horizon = 5 second - -def ExtrapolationPosition (movingObject1, instant,deltaT): - ''' extrapolation hypothesis: constant velocity''' - return movingObject1.getPositionAtInstant(instant) + movingObject1.getVelocityAtInstant(instant). multiply(deltaT) - -def motion (position, velocity, acceleration): - ''' extrapolation hypothesis: constant acceleration''' - from math import atan2,cos,sin - vInit= velocity - vInitial= velocity.norm2() - theta= atan2(velocity.y,velocity.x) - vFinal= vInitial+acceleration - - if acceleration<= 0: - v= max(0,vFinal) - velocity= moving.Point(v* cos(theta),v* sin(theta)) - position= position+ (velocity+vInit). multiply(0.5) - else: - v= min(vLimit,vFinal) - velocity= moving.Point(v* cos(theta),v* sin(theta)) - position= position+ (velocity+vInit). multiply(0.5) - return(position,velocity) - -def motionPET (position, velocity, acceleration, deltaT): - ''' extrapolation hypothesis: constant acceleration for calculating pPET ''' - from math import atan2,cos,sin,fabs - vInit= velocity - vInitial= velocity.norm2() - theta= atan2(velocity.y,velocity.x) - vFinal= vInitial+acceleration * deltaT - if acceleration< 0: - if vFinal> 0: - velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) - position= position+ (vInit+ velocity). multiply(0.5*deltaT) - else: - T= fabs(vInitial/acceleration) - position= position + vInit. multiply(0.5*T) - elif acceleration> 0 : - if vFinal<= vLimit: - velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) - position= position+ (vInit+ velocity). multiply(0.5*deltaT) - else: - time1= fabs((vLimit-vInitial)/acceleration) - velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta)) - position= (position+ (velocity+vInit). multiply(0.5*time1)) + (velocity.multiply (deltaT-time1)) - elif acceleration == 0: - position= position + velocity. multiply(deltaT) - - return position - -def timePET (position, velocity, acceleration, intersectedPoint ): - ''' extrapolation hypothesis: constant acceleration for calculating pPET ''' - from math import atan2,cos,sin,fabs - vInit= velocity - vInitial= velocity.norm2() - theta= atan2(velocity.y,velocity.x) - vFinal= vInitial+acceleration * deltaT - if acceleration< 0: - if vFinal> 0: - velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) - time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) - else: - time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x))) - elif acceleration> 0 : - if vFinal<= vLimit: - velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) - time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) - else: - time1= fabs((vLimit-vInitial)/acceleration) - velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta)) - time2= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) - if time2<=time1: - time= time2 - else: - position2= (position+ (velocity+vInit). multiply(0.5*time1)) - time= time1+fabs((intersectedPoint.x-position2.x)/( velocity.x)) - elif acceleration == 0: - time= fabs((intersectedPoint.x-position.x)/(velocity.x)) - - return time - -def motionSteering (position, velocity, deltaTheta, deltaT ): - ''' extrapolation hypothesis: steering with deltaTheta''' - from math import atan2,cos,sin - vInitial= velocity.norm2() - theta= atan2(velocity.y,velocity.x) - newTheta= theta + deltaTheta - velocity= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta)) - position= position+ (velocity). multiply(deltaT) - return position - -def MonteCarlo(movingObject1,movingObject2, instant): - ''' Monte Carlo Simulation : estimate the probability of collision''' - from random import uniform - from math import pow, sqrt, sin, cos,atan2 - N=1000 - ProbOfCollision = 0 - for n in range (1, N): - # acceleration limit - acc1 = uniform(-0.040444,0) - acc2 = uniform(-0.040444,0) - p1= movingObject1.getPositionAtInstant(instant) - p2= movingObject2.getPositionAtInstant(instant) - v1= movingObject1.getVelocityAtInstant(instant) - v2= movingObject2.getVelocityAtInstant(instant) - distance= (p1-p2).norm2() - distanceThreshold= 1.8 - t=1 - while distance > distanceThreshold and t <= deltaT: - # Extrapolation position - (p1,v1) = motion(p1,v1,acc1) - (p2,v2) = motion(p2,v2,acc2) - distance= (p1-p2).norm2() - if distance <=distanceThreshold: - ProbOfCollision= ProbOfCollision+1 - t+=1 - POC= float(ProbOfCollision)/N - return POC - -def velocitySteering(velocity,steering): - from math import atan2,cos,sin - vInitial= velocity.norm2() - theta= atan2(velocity.y,velocity.x) - newTheta= theta + steering - v= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta)) - return v - -def MonteCarloSteering(movingObject1,movingObject2, instant,steering1,steering2): - ''' Monte Carlo Simulation : estimate the probability of collision in case of steering''' - from random import uniform - from math import pow, sqrt, sin, cos,atan2 - N=1000 - L= 2.4 - ProbOfCollision = 0 - for n in range (1, N): - # acceleration limit - acc1 = uniform(-0.040444,0) - acc2 = uniform(-0.040444,0) - p1= movingObject1.getPositionAtInstant(instant) - p2= movingObject2.getPositionAtInstant(instant) - vInit1= movingObject1.getVelocityAtInstant(instant) - v1= velocitySteering (vInit1,steering1) - vInit2= movingObject2.getVelocityAtInstant(instant) - v2= velocitySteering (vInit2,steering2) - distance= (p1-p2).norm2() - distanceThreshold= 1.8 - t=1 - while distance > distanceThreshold and t <= deltaT: - # Extrapolation position - (p1,v1) = motion(p1,v1,acc1) - (p2,v2) = motion(p2,v2,acc2) - distance= (p1-p2).norm2() - if distance <=distanceThreshold: - ProbOfCollision= ProbOfCollision+1 - t+=1 - POC= float(ProbOfCollision)/N - return POC - - +#! /usr/bin/env python +'''Library for moving object extrapolation hypotheses''' + +import sys + +import moving + +# Default values: to remove because we cannot tweak that from a script where the value may be different +FPS= 25 # No. of frame per second (FPS) +vLimit= 25/FPS #assume limit speed is 90km/hr = 25 m/sec +deltaT= FPS*5 # extrapolatation time Horizon = 5 second + +def motion (position, velocity, acceleration): + ''' extrapolation hypothesis: constant acceleration''' + from math import atan2,cos,sin + vInit= velocity + vInitial= velocity.norm2() + theta= atan2(velocity.y,velocity.x) + vFinal= vInitial+acceleration + + if acceleration<= 0: + v= max(0,vFinal) + velocity= moving.Point(v* cos(theta),v* sin(theta)) + position= position+ (velocity+vInit). multiply(0.5) + else: + v= min(vLimit,vFinal) + velocity= moving.Point(v* cos(theta),v* sin(theta)) + position= position+ (velocity+vInit). multiply(0.5) + return(position,velocity) + +def motionPET (position, velocity, acceleration, deltaT): + ''' extrapolation hypothesis: constant acceleration for calculating pPET ''' + from math import atan2,cos,sin,fabs + vInit= velocity + vInitial= velocity.norm2() + theta= atan2(velocity.y,velocity.x) + vFinal= vInitial+acceleration * deltaT + if acceleration< 0: + if vFinal> 0: + velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) + position= position+ (vInit+ velocity). multiply(0.5*deltaT) + else: + T= fabs(vInitial/acceleration) + position= position + vInit. multiply(0.5*T) + elif acceleration> 0 : + if vFinal<= vLimit: + velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) + position= position+ (vInit+ velocity). multiply(0.5*deltaT) + else: + time1= fabs((vLimit-vInitial)/acceleration) + velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta)) + position= (position+ (velocity+vInit). multiply(0.5*time1)) + (velocity.multiply (deltaT-time1)) + elif acceleration == 0: + position= position + velocity. multiply(deltaT) + + return position + +def timePET (position, velocity, acceleration, intersectedPoint ): + ''' extrapolation hypothesis: constant acceleration for calculating pPET ''' + from math import atan2,cos,sin,fabs + vInit= velocity + vInitial= velocity.norm2() + theta= atan2(velocity.y,velocity.x) + vFinal= vInitial+acceleration * deltaT + if acceleration< 0: + if vFinal> 0: + velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) + time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) + else: + time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x))) + elif acceleration> 0 : + if vFinal<= vLimit: + velocity= moving.Point(vFinal* cos(theta),vFinal* sin(theta)) + time= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) + else: + time1= fabs((vLimit-vInitial)/acceleration) + velocity= moving.Point(vLimit* cos(theta),vLimit* sin(theta)) + time2= fabs((intersectedPoint.x-position.x)/(0.5*(vInit.x+ velocity.x))) + if time2<=time1: + time= time2 + else: + position2= (position+ (velocity+vInit). multiply(0.5*time1)) + time= time1+fabs((intersectedPoint.x-position2.x)/( velocity.x)) + elif acceleration == 0: + time= fabs((intersectedPoint.x-position.x)/(velocity.x)) + + return time + +def motionSteering (position, velocity, deltaTheta, deltaT ): + ''' extrapolation hypothesis: steering with deltaTheta''' + from math import atan2,cos,sin + vInitial= velocity.norm2() + theta= atan2(velocity.y,velocity.x) + newTheta= theta + deltaTheta + velocity= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta)) + position= position+ (velocity). multiply(deltaT) + return position + +def MonteCarlo(movingObject1,movingObject2, instant): + ''' Monte Carlo Simulation : estimate the probability of collision''' + from random import uniform + from math import pow, sqrt, sin, cos,atan2 + N=1000 + ProbOfCollision = 0 + for n in range (1, N): + # acceleration limit + acc1 = uniform(-0.040444,0) + acc2 = uniform(-0.040444,0) + p1= movingObject1.getPositionAtInstant(instant) + p2= movingObject2.getPositionAtInstant(instant) + v1= movingObject1.getVelocityAtInstant(instant) + v2= movingObject2.getVelocityAtInstant(instant) + distance= (p1-p2).norm2() + distanceThreshold= 1.8 + t=1 + while distance > distanceThreshold and t <= deltaT: + # Extrapolation position + (p1,v1) = motion(p1,v1,acc1) + (p2,v2) = motion(p2,v2,acc2) + distance= (p1-p2).norm2() + if distance <=distanceThreshold: + ProbOfCollision= ProbOfCollision+1 + t+=1 + POC= float(ProbOfCollision)/N + return POC + +def velocitySteering(velocity,steering): + from math import atan2,cos,sin + vInitial= velocity.norm2() + theta= atan2(velocity.y,velocity.x) + newTheta= theta + steering + v= moving.Point(vInitial* cos(newTheta),vInitial* sin(newTheta)) + return v + +def MonteCarloSteering(movingObject1,movingObject2, instant,steering1,steering2): + ''' Monte Carlo Simulation : estimate the probability of collision in case of steering''' + from random import uniform + from math import pow, sqrt, sin, cos,atan2 + N=1000 + L= 2.4 + ProbOfCollision = 0 + for n in range (1, N): + # acceleration limit + acc1 = uniform(-0.040444,0) + acc2 = uniform(-0.040444,0) + p1= movingObject1.getPositionAtInstant(instant) + p2= movingObject2.getPositionAtInstant(instant) + vInit1= movingObject1.getVelocityAtInstant(instant) + v1= velocitySteering (vInit1,steering1) + vInit2= movingObject2.getVelocityAtInstant(instant) + v2= velocitySteering (vInit2,steering2) + distance= (p1-p2).norm2() + distanceThreshold= 1.8 + t=1 + while distance > distanceThreshold and t <= deltaT: + # Extrapolation position + (p1,v1) = motion(p1,v1,acc1) + (p2,v2) = motion(p2,v2,acc2) + distance= (p1-p2).norm2() + if distance <=distanceThreshold: + ProbOfCollision= ProbOfCollision+1 + t+=1 + POC= float(ProbOfCollision)/N + return POC + +
--- a/python/moving.py Fri Jul 13 17:30:25 2012 -0400 +++ b/python/moving.py Mon Jul 16 04:57:35 2012 -0400 @@ -508,6 +508,11 @@ indices = self.positions.getIntersections(p1, p2) return [t+self.getFirstInstant() for t in indices] + def predictPosition(self, instant, deltaT, externalAcceleration = Point(0,0)): + '''Predicts the position of object at instant+deltaT, + at constant speed''' + return self.getPositionAtInstant(instant) + self.getVelocityAtInstant(instant).multiply(deltaT) + externalAcceleration.multiply(deltaT**2) + @staticmethod def collisionCourseDotProduct(movingObject1, movingObject2, instant): 'A positive result indicates that the road users are getting closer'