Mercurial Hosting > traffic-intelligence

comparison scripts/test-compute-object-position-from-features.py @ 998:933670761a57

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
updated code to python 3 (tests pass and scripts run, but non-executed parts of code are probably still not correct)
author Nicolas Saunier <nicolas.saunier@polymtl.ca>
date Sun, 27 May 2018 23:22:48 -0400
parents 56cc8a1f7082
children
comparison
equal deleted inserted replaced
997:4f3387a242a1 998:933670761a57
1 #!/usr/bin/env python 1 #!/usr/bin/env python3
2 2
3 import sys 3 import sys
4 4
5 import matplotlib.mlab as pylab 5 import matplotlib.mlab as pylab
6 import matplotlib.pyplot as plt 6 import matplotlib.pyplot as plt
68 return newTraj 68 return newTraj
69 69
70 # TODO version median: conversion to large matrix will not work, have to do it frame by frame 70 # TODO version median: conversion to large matrix will not work, have to do it frame by frame
71 71
72 def kalmanFilter(positions, velocities, processNoiseCov, measurementNoiseCov): 72 def kalmanFilter(positions, velocities, processNoiseCov, measurementNoiseCov):
73 kalman=cv.CreateKalman(6, 4) 73 kalman=cv2.CreateKalman(6, 4)
74 kalman.transition_matrix[0,2]=1 74 kalman.transition_matrix[0,2]=1
75 kalman.transition_matrix[0,4]=1./2 75 kalman.transition_matrix[0,4]=1./2
76 kalman.transition_matrix[1,3]=1 76 kalman.transition_matrix[1,3]=1
77 kalman.transition_matrix[1,5]=1./2 77 kalman.transition_matrix[1,5]=1./2
78 kalman.transition_matrix[2,4]=1 78 kalman.transition_matrix[2,4]=1
95 kalman.state_post[5,0]=a.y 95 kalman.state_post[5,0]=a.y
96 96
97 filteredPositions = moving.Trajectory() 97 filteredPositions = moving.Trajectory()
98 filteredVelocities = moving.Trajectory() 98 filteredVelocities = moving.Trajectory()
99 measurement = cv.CreateMat(4,1,cv.CV_32FC1) 99 measurement = cv.CreateMat(4,1,cv.CV_32FC1)
100 for i in xrange(positions.length()): 100 for i in range(positions.length()):
101 cv.KalmanPredict(kalman) # no control 101 kalman.predict() # no control
102 p = positions[i] 102 p = positions[i]
103 v = velocities[i] 103 v = velocities[i]
104 measurement[0,0] = p.x 104 measurement[0,0] = p.x
105 measurement[1,0] = p.y 105 measurement[1,0] = p.y
106 measurement[2,0] = v.x 106 measurement[2,0] = v.x
107 measurement[3,0] = v.y 107 measurement[3,0] = v.y
108 cv.KalmanCorrect(kalman, measurement) 108 kalman.correct(measurement)
109 filteredPositions.addPositionXY(kalman.state_post[0,0], kalman.state_post[1,0]) 109 filteredPositions.addPositionXY(kalman.state_post[0,0], kalman.state_post[1,0])
110 filteredVelocities.addPositionXY(kalman.state_post[2,0], kalman.state_post[3,0]) 110 filteredVelocities.addPositionXY(kalman.state_post[2,0], kalman.state_post[3,0])
111 111
112 return (filteredPositions, filteredVelocities) 112 return (filteredPositions, filteredVelocities)
113 113