Mercurial Hosting > traffic-intelligence

annotate python/compute-object-from-features.py @ 106:ce4cb46b3603

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
added kalman filtering and rearranged functions
author Nicolas Saunier <nico@confins.net>
date Thu, 14 Jul 2011 20:05:01 -0400
parents 9844c69d8fa2
children 04a874e1f19f
Ignore whitespace changes - Everywhere: Within whitespace: At end of lines:
rev   line source
103
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
1 #!/usr/bin/env python
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
2
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
3 import sys
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
4
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
5 import matplotlib.mlab as pylab
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
6 import matplotlib.pyplot as plt
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
7 import numpy as np
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
8
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
9 import cv
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
10 import utils
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
11 import cvutils
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
12 import ubc_utils
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
13 import moving
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
14
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
15 # use something like getopt to manage arguments if necessary
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
16
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
17 if len(sys.argv) < 3:
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
18 print('Usage: {0} <video-filename> <n-objects>'.format(sys.argv[0]))
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
19 sys.exit()
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
20
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
21 if sys.argv[1].endswith('.avi'):
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
22 videoFilenamePrefix = utils.removeExtension(sys.argv[1],'.')
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
23 else:
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
24 videoFilenamePrefix = sys.argv[1]
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
25
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
26 objectNum = int(sys.argv[2])
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
27
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
28 objects = ubc_utils.loadTrajectories(videoFilenamePrefix+'-objects.txt', objectNum+1)
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
29 obj = objects[objectNum]
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
30 features = ubc_utils.loadTrajectories(videoFilenamePrefix+'-features.txt', max(obj.featureNumbers)+1)
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
31 h = np.loadtxt(videoFilenamePrefix+'-homography.txt')
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
32
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
33 invh = cvutils.invertHomography(h)
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
34
106
ce4cb46b3603 added kalman filtering and rearranged functions
Nicolas Saunier <nico@confins.net>
parents: 105
diff changeset
35 def computeGroundTrajectory(features, homography, timeInterval = None):
ce4cb46b3603 added kalman filtering and rearranged functions
Nicolas Saunier <nico@confins.net>
parents: 105
diff changeset
36 '''Computes a trajectory for the set of features as the closes point to the ground
ce4cb46b3603 added kalman filtering and rearranged functions
Nicolas Saunier <nico@confins.net>
parents: 105
diff changeset
37 using the homography in image space'''
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
38 if not timeInterval:
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
39 raise Exception('not implemented') # compute from the features
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
40
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
41 yCoordinates = -np.ones((len(features),int(timeInterval.length())))
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
42 for i,f in enumerate(features):
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
43 traj = f.getPositions().asArray()
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
44 imgTraj = cvutils.projectArray(homography, traj)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
45 yCoordinates[i,f.getFirstInstant()-timeInterval.first:f.getLastInstant()+1-timeInterval.first] = imgTraj[1,:]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
46
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
47 indices = np.argmax(yCoordinates,0)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
48 newTraj = moving.Trajectory()
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
49 for j,idx in enumerate(indices):
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
50 newTraj.addPosition(features[idx].getPositionAtInstant(j+timeInterval.first))
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
51 #newVelocities.addPosition(features[obj.featureNumbers[idx]].getVelocityAtInstant(j+obj.getFirstInstant()))
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
52
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
53 return newTraj
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
54
106
ce4cb46b3603 added kalman filtering and rearranged functions
Nicolas Saunier <nico@confins.net>
parents: 105
diff changeset
55 # TODO version median: conversion to large matrix will not work, have to do it frame by frame
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
56
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
57 def kalmanFilter(positions, velocities, processNoiseCov, measurementNoiseCov):
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
58 kalman=cv.CreateKalman(6, 4)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
59 kalman.transition_matrix[0,2]=1
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
60 kalman.transition_matrix[0,4]=1./2
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
61 kalman.transition_matrix[1,3]=1
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
62 kalman.transition_matrix[1,5]=1./2
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
63 kalman.transition_matrix[2,4]=1
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
64 kalman.transition_matrix[3,5]=1
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
65
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
66 cv.SetIdentity(kalman.measurement_matrix, 1.)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
67 cv.SetIdentity(kalman.process_noise_cov, processNoiseCov)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
68 cv.SetIdentity(kalman.measurement_noise_cov, measurementNoiseCov)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
69 cv.SetIdentity(kalman.error_cov_post, 1.)
103
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
70
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
71 p = positions[0]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
72 v = velocities[0]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
73 v2 = velocities[2]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
74 a = (v2-v).multiply(0.5)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
75 kalman.state_post[0,0]=p.x
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
76 kalman.state_post[1,0]=p.y
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
77 kalman.state_post[2,0]=v.x
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
78 kalman.state_post[3,0]=v.y
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
79 kalman.state_post[4,0]=a.x
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
80 kalman.state_post[5,0]=a.y
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
81
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
82 filteredPositions = moving.Trajectory()
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
83 filteredVelocities = moving.Trajectory()
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
84 measurement = cv.CreateMat(4,1,cv.CV_32FC1)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
85 for i in xrange(positions.length()):
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
86 cv.KalmanPredict(kalman) # no control
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
87 p = positions[i]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
88 v = velocities[i]
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
89 measurement[0,0] = p.x
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
90 measurement[1,0] = p.y
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
91 measurement[2,0] = v.x
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
92 measurement[3,0] = v.y
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
93 cv.KalmanCorrect(kalman, measurement)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
94 filteredPositions.addPositionXY(kalman.state_post[0,0], kalman.state_post[1,0])
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
95 filteredVelocities.addPositionXY(kalman.state_post[2,0], kalman.state_post[3,0])
103
1621b46a1523 started python script to compute the moving object trajectory as the trajectory of the lowest of the features (ie closest to the ground)
Nicolas Saunier <nico@confins.net>
parents:
diff changeset
96
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
97 return (filteredPositions, filteredVelocities)
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
98
106
ce4cb46b3603 added kalman filtering and rearranged functions
Nicolas Saunier <nico@confins.net>
parents: 105
diff changeset
99 lowTrajectory = computeGroundTrajectory([features[i] for i in obj.featureNumbers], invh, obj.getTimeInterval())
ce4cb46b3603 added kalman filtering and rearranged functions
Nicolas Saunier <nico@confins.net>
parents: 105
diff changeset
100 (filteredPositions, filteredVelocities) = kalmanFilter(lowTrajectory, obj.getVelocities(), 0.1, 0.1)
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
101
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
102 plt.clf()
106
ce4cb46b3603 added kalman filtering and rearranged functions
Nicolas Saunier <nico@confins.net>
parents: 105
diff changeset
103 obj.draw('rx-')
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
104 for fnum in obj.featureNumbers: features[fnum].draw()
106
ce4cb46b3603 added kalman filtering and rearranged functions
Nicolas Saunier <nico@confins.net>
parents: 105
diff changeset
105 lowTrajectory.draw('bx-')
ce4cb46b3603 added kalman filtering and rearranged functions
Nicolas Saunier <nico@confins.net>
parents: 105
diff changeset
106 filteredPositions.draw('gx-')
105
9844c69d8fa2 added multiply method to Point
Nicolas Saunier <nico@confins.net>
parents: 103
diff changeset
107 plt.axis('equal')