Mercurial Hosting > traffic-intelligence

changeset 243:e0988a8ace0c

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
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'