def __init__(self, detection, trackId):
super(Tracks, self).__init__()
self.KF = KalmanFilter()
self.KF.predict()
self.KF.correct(np.matrix(detection).reshape(2, 1))
self.trace = deque(maxlen=20)
self.prediction = detection.reshape(1, 2)
self.trackId = trackId
self.skipped_frames = 0
def updateState(self, measurements, probabilities, p0):
x_p = self.x_p
p_p = self.p_p
if measurements.size > 0:
y_u, y_i = self.__getInnovation__(x_p, measurements, probabilities)
s_u = KalmanFilter.__getInnovationCovariance__(self, p_p)
k_u = KalmanFilter.__getKalmanGain__(self, s_u, p_p)
x_u = KalmanFilter.__getUpdatedState__(self, x_p, k_u, y_u)
p_u = self.__getStateCovariance__(k_u, p_p, s_u, y_u, y_i,
probabilities, p0)
else:
self.x_u = x_p
self.p_u = p_p
self.measurement = measurements
self.probabilities = probabilities
def __init__(self,
A=None,
H=None,
Q=None,
R=None,
G=None,
X_0=None,
threshold=100):
KalmanFilter.__init__(self, A, H, Q, R, G, X_0)
self.threshold = threshold
self.sphereVolume = self.__getSphereVolume__(H.shape[0])
self.__initializeInnovationCovariance__()
self.age = 1
self.totalVisibleCount = 0
self.consecutiveInvisibleCount = 0
self.detectionProbability = 0.90
def __getInnovation__(self, prediction, measurements, probabilities):
y_u = 0
y_i = []
for nm, measurement in enumerate(measurements):
y_u_i = np.array(
KalmanFilter.__getInnovation__(self, prediction, measurement))
y_u += probabilities[nm] * y_u_i
y_i.append(y_u_i)
return np.matrix(y_u).T, np.matrix(y_i).T
def __init__(self,
respond = None,
regressors = None,
intercept = False,
Sigma = None,
sigma = None,
initBeta = None,
initVariance = None,
Phi = None,
**args):
"""
:param respond: Dependent time series
:type respond: TimeSeriesFrame<double>
:param regressors: Independent time serieses
:type regressors: TimeSeriesFrame<double>
:param intercept: include/exclude intercept in the regression
:type intercept: boolean
"""
KalmanFilter.__init__(self, respond, regressors, intercept, Sigma, sigma, initBeta, initVariance, Phi, **args)
def __init__(self,
respond=None,
regressors=None,
intercept=False,
Sigma=None,
sigma=None,
initBeta=None,
initVariance=None,
Phi=None,
**args):
"""
:param respond: Dependent time series
:type respond: TimeSeriesFrame<double>
:param regressors: Independent time serieses
:type regressors: TimeSeriesFrame<double>
:param intercept: include/exclude intercept in the regression
:type intercept: boolean
"""
KalmanFilter.__init__(self, respond, regressors, intercept, Sigma,
sigma, initBeta, initVariance, Phi, **args)
def __init__(self, prediction, trackIdCount):
"""Initialize variables used by Track class
Args:
prediction: predicted centroids of object to be tracked
trackIdCount: identification of each track object
Return:
None
"""
self.track_id = trackIdCount # identification of each track object
self.KF = KalmanFilter() # KF instance to track this object
self.prediction = np.asarray(prediction) # predicted centroids (x,y)
self.skipped_frames = 0 # number of frames skipped undetected
self.trace = [] # trace path
class Tracks(object):
"""docstring for Tracks"""
def __init__(self, detection, trackId):
super(Tracks, self).__init__()
self.KF = KalmanFilter()
self.KF.predict()
self.KF.correct(np.matrix(detection).reshape(2, 1))
self.trace = deque(maxlen=20)
self.prediction = detection.reshape(1, 2)
self.trackId = trackId
self.skipped_frames = 0
def predict(self, detection):
self.prediction = np.array(self.KF.predict()).reshape(1, 2)
self.KF.correct(np.matrix(detection).reshape(2, 1))
class TrackingObject(object): """ TrackingObject: maintains and updates all the information for one tracked object """ def __init__(self, detection, trackId): super(TrackingObject, self).__init__() self.KF = KalmanFilter() self.KF.predict() self.KF.correct(np.matrix(detection).reshape(2,1)) self.trace = deque(maxlen=20) self.prediction = detection.reshape(1,2) self.trackId = trackId self.skipped_frames = 0 def predict_and_correct(self, measurement): self.prediction = np.array(self.KF.predict()).reshape(1,2) self.KF.correct(np.matrix(measurement).reshape(2,1))
x = est_rbt_x[:,
est_k] # last state estimate vector from robot frame
P = est_rbt_P[:, :, est_k] # last covariance matrix
Q = est_rbt_Q # process noise covariance matrix
R = est_rbt_R # measurement noise covariance
H = est_rbt_H # observation matrix
z = est_rbt_m[:, est_k] # measurement vector
u = matrix(
[[0], [0], [0]]
) # control input vector (don't give kalman filter knowledge about thruster inputs)
A = est_rbt_A
B = est_rbt_B
state = KalmanFilter(x, P, z, u, A, B, Q, R, H)
x, P = state.predict(x, P, u)
x, K, P = state.update(x, P, z)
# print('x', x)
# # print('P', P)
# # print('Q', Q)
# # print('R', R)
# # print('H', H)
# # print('z', z)
# # print('u', u)
# # print('K', K)
est_rbt_x[:, est_k + 1] = x
est_rbt_L[:, :, est_k + 1] = K
est_rbt_P[:, :, est_k + 1] = P
def main():
# Pretty videos:
fourcc = cv2.VideoWriter_fourcc(*'mp4v')
kyleOutputVideo = cv2.VideoWriter('../UntrackedFiles/BallOutputKyle.mp4',
fourcc, 60.0, (1920, 1080))
meganOutputVideo = cv2.VideoWriter('../UntrackedFiles/BallOutputMegan.mp4',
fourcc, 60.0, (1920, 1080))
meganFilename = '../UntrackedFiles/stereoClip5_Megan.mov'
kyleFilename = '../UntrackedFiles/stereoClip5_Kyle.mov'
vrMegan1 = VideoReader(meganFilename)
vrMegan2 = VideoReader(meganFilename)
vrKyle1 = VideoReader(kyleFilename)
vrKyle2 = VideoReader(kyleFilename)
numFrameForward = 5
vrMegan2.setNextFrame(numFrameForward)
vrKyle2.setNextFrame(numFrameForward)
# find court corners:
cfKyle = CourtFinder()
cfMegan = CourtFinder()
numFrames = int(vrMegan1.getNumFrames())
vrMegan1.setNextFrame(int(numFrames / 2))
ret, frame = vrMegan1.readFrame()
cfMegan.FindCourtCorners(frame, 0)
vrKyle1.setNextFrame(int(numFrames / 2))
ret, frame = vrKyle1.readFrame()
cfKyle.FindCourtCorners(frame, 0)
if (not cfMegan.found_corners) or not (cfKyle.found_corners):
print "Couldn't find the court. Exiting."
return
# reset frame index to beginning
vrMegan1.setNextFrame(0)
vrKyle1.setNextFrame(0)
frameNum = 1
meganCam = Camera("megan", cfMegan.corners_sort)
kyleCam = Camera("kyle", cfKyle.corners_sort)
# make a ball finder
bf = BallFinder()
kf = KalmanFilter(vrMegan1.framerate)
csvData = []
while (True):
ret1, kyleFrame1 = vrKyle1.readFrame()
ret2, kyleFrame2, = vrKyle2.readFrame()
ret3, meganFrame1 = vrMegan1.readFrame()
ret4, meganFrame2, = vrMegan2.readFrame()
if not (ret1) or not (ret2) or not (ret3) or not (ret4):
print 'Ending after', frameNum - 1, 'frames.'
break
kyleMask, kyleRays = getBallCandidateRays(bf, kyleCam, kyleFrame1,
kyleFrame2)
meganMask, meganRays = getBallCandidateRays(bf, meganCam, meganFrame1,
meganFrame2)
# check all candidate rays for candidate balls
minDist = 1000000
# TODO set inf
ballPt = []
# all ball points and distances
threshDist = 0.2
# rays must be within .1 m of each other for intersect
ballCandidates = []
candidateCertainty = []
for kyleRay in kyleRays:
for meganRay in meganRays:
pt, dist, _D, _E = IntersectRays(kyleRay, meganRay)
if dist < threshDist and pt[1] < 3.5:
# don't include candidates clearly not valid intersect points
# also don't include candidates that are clearly too high to be the ball
courtBuffer = 2
if pt[0] < Camera.HALF_COURT_X + courtBuffer and pt[
0] > -Camera.HALF_COURT_X - courtBuffer:
if pt[2] < Camera.HALF_COURT_Z + 0.6: # and pt[2] > -Camera.HALF_COURT_Z - 0:
ballCandidates.append(pt)
candidateCertainty.append(dist)
if dist < minDist:
minDist = dist
ballPt = pt
kf.processMeas(ballCandidates, candidateCertainty)
# ========== CSV and VIDEO output ===========
csvTuple = list()
csvTuple.append(frameNum)
if np.linalg.norm(kf.sigma_k, 'fro') < 100: # valid result
# Format the tuple for successful reading
csvTuple.append(1)
posVel = np.reshape(kf.mu_k, (1, 6))[0]
posVel = np.round(posVel, 3)
for val in posVel:
csvTuple.append(val)
for val in kyleCam.ConvertWorldToImagePosition(posVel[0:3]):
csvTuple.append(val)
for val in meganCam.ConvertWorldToImagePosition(posVel[0:3]):
csvTuple.append(val)
# Videos
kyleOutFrame = kyleFrame1.copy()
bf.ballPixelLoc = kyleCam.ConvertWorldToImagePosition(posVel[0:3])
kyleOutFrame = cfKyle.drawCornersOnFrame(kyleOutFrame)
kyleOutFrame = bf.drawBallOnFrame(kyleOutFrame)
kyleOutFrame |= kyleMask
kyleOutputVideo.write(kyleOutFrame)
meganOutFrame = meganFrame1.copy()
meganOutFrame |= meganMask
bf.ballPixelLoc = meganCam.ConvertWorldToImagePosition(posVel[0:3])
meganOutFrame = cfMegan.drawCornersOnFrame(meganOutFrame)
meganOutFrame = bf.drawBallOnFrame(meganOutFrame)
meganOutputVideo.write(meganOutFrame)
else:
# Format the tuple for unsuccessful reading
csvTuple.append(0)
csvData.append(list(csvTuple))
print csvTuple
frameNum += 1
# <END WHILE LOOP>
with open('../UntrackedFiles/ballEst.csv', 'wb') as csvfile:
filewriter = csv.writer(csvfile, delimiter=',')
filewriter.writerows(csvData)
vrKyle1.close()
vrKyle2.close()
vrMegan1.close()
vrMegan2.close()
kyleOutputVideo.release()
meganOutputVideo.release()
obs = 0.01
(x, y) = dataGen.generateSamplePointsGG(samplingTime, numSamples, dt, x0, procs, obs)
print "samplePoints generated"
finalTime = samplingTime * (numSamples - 1)
pf = ParticleFilter(oscillator, x0.size)
pf.runFilter(y, samplingTime, dtf, procs, obs)
pltWorker.plotPath(x, finalTime, 0.5)
pltWorker.plotMarkers(y, finalTime, mk="*", ms=10, a=0.4)
pltWorker.plotPFMarkers(pf.x, pf.w, finalTime)
pltWorker.save("dists.pdf")
pltWorker.clear()
z = pf.getAveragePath()
pltWorker.plotPath(x, finalTime, 0.5)
pltWorker.plotMarkers(y, finalTime)
pltWorker.plotMarkers(z, finalTime, mk="x", ms=8)
pltWorker.save("sir.pdf")
pltWorker.clear()
ac = dataGen.calculateAccuracy(x, z)
print "Particle filter error:", ac
kf = KalmanFilter(oscillator, x0.size)
kf.runFilter(y, samplingTime, dtf, procs, obs)
k = kf.getAveragePath()
pltWorker.plotPath(x, finalTime, 0.5)
pltWorker.plotMarkers(y, finalTime)
pltWorker.plotMarkers(k, finalTime, mk="x", ms=8)
pltWorker.save("kalman.pdf")
ac = dataGen.calculateAccuracy(x, k)
print "Kalman filter error:", ac
obs = 0.01
(x, y) = dataGen.generateSamplePointsGG(samplingTime, numSamples, dt, x0,
procs, obs)
print 'samplePoints generated'
finalTime = samplingTime * (numSamples - 1)
pf = ParticleFilter(oscillator, x0.size)
pf.runFilter(y, samplingTime, dtf, procs, obs)
pltWorker.plotPath(x, finalTime, 0.5)
pltWorker.plotMarkers(y, finalTime, mk='*', ms=10, a=0.4)
pltWorker.plotPFMarkers(pf.x, pf.w, finalTime)
pltWorker.save("dists.pdf")
pltWorker.clear()
z = pf.getAveragePath()
pltWorker.plotPath(x, finalTime, 0.5)
pltWorker.plotMarkers(y, finalTime)
pltWorker.plotMarkers(z, finalTime, mk='x', ms=8)
pltWorker.save('sir.pdf')
pltWorker.clear()
ac = dataGen.calculateAccuracy(x, z)
print "Particle filter error:", ac
kf = KalmanFilter(oscillator, x0.size)
kf.runFilter(y, samplingTime, dtf, procs, obs)
k = kf.getAveragePath()
pltWorker.plotPath(x, finalTime, 0.5)
pltWorker.plotMarkers(y, finalTime)
pltWorker.plotMarkers(k, finalTime, mk='x', ms=8)
pltWorker.save('kalman.pdf')
ac = dataGen.calculateAccuracy(x, k)
print "Kalman filter error:", ac
F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]])
Q = np.diag([0, acceleration_variance, 0, acceleration_variance])
H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])
R = np.array([[meas_variance, 0], [0, meas_variance]])
# ---------------------------------------------------------------------------
# ---------------------------------------------------------------------------
# Initializing Kalman Filter
kf = KalmanFilter(initial_state=x,
initial_covariance=P,
process_model=F,
process_noise=Q,
measurement_function=H,
measurement_covariance=R,
no_of_state_variables=no_of_state_variables,
no_of_state_measurements=no_of_state_measurements)
step = 0 # step counter, used for simulation of abscent measurements only
# Loop that runs throughout the time array defined previously
for t in time:
#Simulate real speed
real_speed_x = initial_speed_x + np.random.randn() * np.sqrt(
acceleration_variance)
real_speed_y = initial_speed_y + np.random.randn() * np.sqrt(
acceleration_variance)