def undistortTrajectoryFromCVMapping(map1, map2, t):
'''test 'perfect' inversion'''
undistortedTrajectory = moving.Trajectory()
for i,p in enumerate(t):
res = undistortedCoordinates(map1, map2, p.x,p.y)
if not isnan(res).any():
undistortedTrajectory.addPositionXY(res[0], res[1])
else:
print('{} {} {}'.format(i,p,res))
return undistortedTrajectory
def kalmanFilter(positions, velocities, processNoiseCov, measurementNoiseCov):
kalman = cv.CreateKalman(6, 4)
kalman.transition_matrix[0, 2] = 1
kalman.transition_matrix[0, 4] = 1. / 2
kalman.transition_matrix[1, 3] = 1
kalman.transition_matrix[1, 5] = 1. / 2
kalman.transition_matrix[2, 4] = 1
kalman.transition_matrix[3, 5] = 1
cv.SetIdentity(kalman.measurement_matrix, 1.)
cv.SetIdentity(kalman.process_noise_cov, processNoiseCov)
cv.SetIdentity(kalman.measurement_noise_cov, measurementNoiseCov)
cv.SetIdentity(kalman.error_cov_post, 1.)
p = positions[0]
v = velocities[0]
v2 = velocities[2]
a = (v2 - v).multiply(0.5)
kalman.state_post[0, 0] = p.x
kalman.state_post[1, 0] = p.y
kalman.state_post[2, 0] = v.x
kalman.state_post[3, 0] = v.y
kalman.state_post[4, 0] = a.x
kalman.state_post[5, 0] = a.y
filteredPositions = moving.Trajectory()
filteredVelocities = moving.Trajectory()
measurement = cv.CreateMat(4, 1, cv.CV_32FC1)
for i in xrange(positions.length()):
cv.KalmanPredict(kalman) # no control
p = positions[i]
v = velocities[i]
measurement[0, 0] = p.x
measurement[1, 0] = p.y
measurement[2, 0] = v.x
measurement[3, 0] = v.y
cv.KalmanCorrect(kalman, measurement)
filteredPositions.addPositionXY(kalman.state_post[0, 0],
kalman.state_post[1, 0])
filteredVelocities.addPositionXY(kalman.state_post[2, 0],
kalman.state_post[3, 0])
return (filteredPositions, filteredVelocities)
def computeMedianTrajectory(features, timeInterval=None):
if not timeInterval:
raise Exception('not implemented') # compute from the features
newTraj = moving.Trajectory()
for t in timeInterval:
points = []
for f in features:
if f.existsAtInstant(t):
points.append(f.getPositionAtInstant(t).aslist())
med = np.median(np.array(points), 0)
newTraj.addPositionXY(med[0], med[1])
return newTraj
def buildFeature(obj, featureID, num=1):
featureList = getFeatures(obj, featureID)
tmp = {}
delta = {}
for i in featureList:
for t in xrange(i[1], i[2] + 1):
tmp[t] = [i[0], i[3]]
newTraj = moving.Trajectory()
for instant in obj.getTimeInterval():
newTraj.addPosition(
tmp[instant][0].getPositionAtInstant(instant) + tmp[instant][1])
newFeature = moving.MovingObject(
num, timeInterval=obj.getTimeInterval(), positions=newTraj)
return newFeature
def computeGroundTrajectory(features, homography, timeInterval=None):
'''Computes a trajectory for the set of features as the closes point to the ground
using the homography in image space'''
if not timeInterval:
raise Exception('not implemented') # compute from the features
yCoordinates = -np.ones((len(features), int(timeInterval.length())))
for i, f in enumerate(features):
traj = f.getPositions().asArray()
imgTraj = cvutils.projectArray(homography, traj)
yCoordinates[i,
f.getFirstInstant() -
timeInterval.first:f.getLastInstant() + 1 -
timeInterval.first] = imgTraj[1, :]
indices = np.argmax(yCoordinates, 0)
newTraj = moving.Trajectory()
for j, idx in enumerate(indices):
newTraj.addPosition(
features[idx].getPositionAtInstant(j + timeInterval.first))
#newVelocities.addPosition(features[obj.featureNumbers[idx]].getVelocityAtInstant(j+obj.getFirstInstant()))
return newTraj
def smoothObject(obj,
newNum,
minLengthParam=0.7,
smoothing=False,
plotResults=True,
halfWidth=3,
_computeVelocities=True,
optimize=True,
create=False):
'''Computes a smoother trajectory for the object
and optionnally smoother velocities
The object should have its features in obj.features
TODO: check whether features are necessary'''
if not obj.hasFeatures():
print(
'Object {} has an empty list of features: please load and add them using obj.setFeatures(features)'
.format(obj.getNum()))
from sys import exit
exit()
featureList = [
i for i, f in enumerate(obj.getFeatures())
if f.length() >= minLengthParam * obj.length()
]
if featureList == []:
featureList.append(
utils.argmaxDict(
{i: f.length()
for i, f in enumerate(obj.getFeatures())}))
create = True
newObjects = []
for featureID in featureList: # featureID should be the index in the list of obj.features
newObjects.append(
smoothObjectTrajectory(obj,
featureID,
newNum,
smoothing=smoothing,
halfWidth=halfWidth,
create=create))
newTranslated = moving.Trajectory()
newInterval = []
for t in obj.getTimeInterval():
xCoord = []
yCoord = []
for i in newObjects:
if i.existsAtInstant(t):
p1 = i.getPositionAtInstant(t)
xCoord.append(p1.x)
yCoord.append(p1.y)
if xCoord != []:
tmp = moving.Point(median(xCoord), median(yCoord))
newInterval.append(t)
newTranslated.addPosition(tmp)
newObj = moving.MovingObject(newNum,
timeInterval=moving.TimeInterval(
min(newInterval), max(newInterval)),
positions=newTranslated)
if _computeVelocities:
tmpTraj = moving.Trajectory()
velocities = computeVelocities(newObj, True, 5)
for i in sorted(velocities.keys()):
tmpTraj.addPosition(velocities[i])
newObj.velocities = tmpTraj
else:
newObj.velocities = obj.velocities
if optimize:
csj1 = sumSquaredJerk(obj, fromPosition=True)
csj2 = sumSquaredJerk(newObj, fromPosition=True)
if csj1 < csj2:
newObj = obj
newObj.velocities = obj.velocities
if _computeVelocities and csj1 >= csj2:
csj3 = sumSquaredJerk(obj, fromPosition=False)
csj4 = sumSquaredJerk(newObj, fromPosition=False)
if csj4 <= csj3:
newObj.velocities = obj.velocities
newObj.featureNumbers = obj.featureNumbers
newObj.features = obj.getFeatures()
newObj.userType = obj.userType
if plotResults:
plt.figure()
plt.title('objects_id = {}'.format(obj.num))
for i in featureList:
obj.getFeature(i).plot('cx-')
obj.plot('rx-')
newObj.plot('gx-')
return newObj
def smoothObjectTrajectory(obj,
featureID,
newNum,
smoothing=False,
halfWidth=3,
create=False):
results = []
bearing = {}
if create:
feature = buildFeature(obj, featureID, num=1) # why num=1
else:
feature = obj.getFeature(featureID)
for t in feature.getTimeInterval():
p1 = feature.getPositionAtInstant(t)
p2 = obj.getPositionAtInstant(t)
if t != feature.getLastInstant():
p3 = feature.getPositionAtInstant(t + 1)
else:
p1 = feature.getPositionAtInstant(t - 1)
p3 = feature.getPositionAtInstant(t)
bearing[t] = getBearing(p1, p2, p3)[1]
results.append(getBearing(p1, p2, p3))
medianResults = median(results, 0)
dist = medianResults[0]
angle = medianResults[3]
for i in sorted(bearing.keys()):
bearing[i] = bearing[i] + angle
if smoothing:
bearingInput = []
for i in sorted(bearing.keys()):
bearingInput.append(bearing[i])
import utils
bearingOut = utils.filterMovingWindow(bearingInput, halfWidth)
for t, i in enumerate(sorted(bearing.keys())):
bearing[i] = bearingOut[t]
#solve a smoothing problem in case of big drop in computing bearing (0,360)
for t, i in enumerate(sorted(bearing.keys())):
if i != max(bearing.keys()) and abs(bearingInput[t] -
bearingInput[t + 1]) >= 340:
for x in xrange(max(i - halfWidth, min(bearing.keys())),
min(i + halfWidth, max(bearing.keys())) + 1):
bearing[x] = bearingInput[t - i + x]
translated = moving.Trajectory()
for t in feature.getTimeInterval():
p1 = feature.getPositionAtInstant(t)
p1.x = p1.x + dist * sin(bearing[t] * pi / 180)
p1.y = p1.y + dist * cos(bearing[t] * pi / 180)
translated.addPosition(p1)
#modify first and last un-smoothed positions (half width)
if smoothing:
d1 = translated[halfWidth] - feature.positions[halfWidth]
d2 = translated[-halfWidth - 1] - feature.positions[-halfWidth - 1]
for i in xrange(halfWidth):
p1 = feature.getPositionAt(i) + d1
p2 = feature.getPositionAt(-i - 1) + d2
translated.setPosition(i, p1)
translated.setPosition(-i - 1, p2)
newObj = moving.MovingObject(newNum,
timeInterval=feature.getTimeInterval(),
positions=translated)
return newObj
[features[i] for i in obj.featureNumbers], invh, obj.getTimeInterval())
(filteredPositions, filteredVelocities) = kalmanFilter(groundTrajectory,
obj.getVelocities(),
0.1, 0.1)
#medianTrajectory = computeMedianTrajectory([features[i] for i in obj.featureNumbers], obj.getTimeInterval())
delta = []
for t in obj.getTimeInterval():
p1 = obj.getPositionAtInstant(t)
p2 = groundTrajectory[t - obj.getFirstInstant()]
delta.append((p1 - p2).aslist())
delta = np.median(delta, 0)
translated = moving.Trajectory()
for t in obj.getTimeInterval():
p1 = obj.getPositionAtInstant(t)
p1.x -= delta[0]
p1.y -= delta[1]
translated.addPosition(p1)
plt.clf()
obj.draw('rx-')
for fnum in obj.featureNumbers:
features[fnum].draw()
groundTrajectory.draw('bx-')
filteredPositions.draw('gx-')
translated.draw('kx-')
#medianTrajectory.draw('kx-')
plt.axis('equal')