def create_trackers(self, det_bboxes):
# create trackers initialised by det bboxes
trackers = [KalmanFilter() for _ in range(len(det_bboxes))]
for t, o in zip(trackers, det_bboxes):
t.initialize(o)
self.num_objs += 1
self.tracker_ids[t] = self.num_objs
return trackers
def main():
deltaTime = 0.001
numSteps = 500
F = np.array([[1, 0], [0, 1]]) * 0
B = np.array([[1, 0], [0, 1]])
H = np.array([[1, 0], [0, 1]])
P = np.array([[0, 0], [0, 0]])
p_sigma = 1e-3
o_sigma = 1e-3
Q = np.array([[1, 0], [0, 1]]) * (p_sigma**2)
R = np.array([[1, 0], [0, 1]]) * (o_sigma**2)
t = (np.arange(0, numSteps) * 0.01)
i = np.zeros((2, numSteps))
i[0, 0:] = np.sin(2 * np.pi * 1 * t) * 1
p_n = np.reshape(
np.random.multivariate_normal([0, 0], [[p_sigma, 0], [0, p_sigma]],
numSteps), (2, numSteps))
o_n = np.reshape(
np.random.multivariate_normal([0, 0], [[o_sigma, 0], [0, o_sigma]],
numSteps), (2, numSteps))
x = np.zeros((2, numSteps)) #process output
y = np.zeros((2, numSteps)) #process output
x_pred = np.zeros((2, numSteps))
KF = KalmanFilter(F, B, Q, H, R, P, x[0:, 0])
for t in range(1, numSteps):
x[0:,
t] = B.dot(i[0:, t]) + p_n[0:, t] #process output with process noise
KF.predict(i[0:, t])
x_pred[0:, t], P = KF.getPrediction() #predicting next process state
#print(P)
y[0:, t] = h(x[0:, t]) + o_n[0:, t] #sensor output
KF.update(y[0:, t]) #update Kalman
print(np.sum((y - x_pred)**2) / numSteps)
print(np.sum((x - x_pred)**2) / numSteps)
plt.plot(x[0, 0:], 'r', y[0, 0:], 'b', x_pred[0, 0:], 'g')
plt.show()
def main(): deltaTime=0.01 numSteps=5000 x_0=np.array([[0],[0]]) F=np.array([[1, deltaTime],[0, 1]]) B=np.array([[0, 0.5*deltaTime**2],[0, deltaTime]]) H=np.array([[1, 0],[0, 1]]) P=np.array([[0, 0],[0, 0]]) p_sigma=1e-7 o_sigma=1e-7 Q=np.array([[0.25*deltaTime**4, 0.5*deltaTime**3],[0.5*deltaTime**3, deltaTime**2]])*(p_sigma**2) R=np.array([[1, 0],[0, 1]])*(o_sigma**2) t=(np.arange(0,numSteps)*deltaTime) i=np.zeros((2,numSteps)) i[1,0:]=np.cos(2*np.pi*0.1*t)*10 p_n=np.reshape(np.random.multivariate_normal([0, 0],[[p_sigma,0],[0,p_sigma]],numSteps),(2,numSteps))*0 o_n=np.reshape(np.random.multivariate_normal([0, 0],[[o_sigma,0],[0,o_sigma]],numSteps),(2,numSteps))*0 x=np.zeros((2,numSteps)) y=np.zeros((2,numSteps)) x_pred=np.zeros((2,numSteps)) x[0,0]=0 x[1,0]=0 KF=KalmanFilter(F,B,Q,H,R,P,x[0:,0]) for t in range(1,numSteps): x[0:,t]=F.dot(x[0:,t-1])+B.dot(i[0:,t])+p_n[0:,t] KF.predict(i[0:,t]) x_pred[0:,t],P=KF.getPrediction() #print(P) y[0:,t]=H.dot(x[0:,t])+o_n[0:,t] KF.update(y[0:,t]) print(np.sum((y-x_pred)**2)/numSteps) print(np.sum((x-x_pred)**2)/numSteps) plt.plot(i[1,0:],'k',x[0,0:],'r',y[0,0:],'b', x_pred[0,0:],'g') plt.show()
F = np.eye(34)
F[0:17, 17:] = np.eye(17) * deltaTime
B = np.eye(34)
H = np.eye(34)
P = np.zeros((34, 34))
p_sigma = std
o_sigma = std
Q = np.diag(p_sigma**2)
R = np.diag(o_sigma**2)
jointStatesEstimation = []
KF = KalmanFilter(F, B, Q, H, R, P, mean)
def callback(data):
#print("I heard "+str(np.array(np.concatenate((data.position,data.velocity)))))
#jointStates.append(data)
global num
num += 1
global KF
KF.predict(np.zeros(34)) #Needs revision
[pos, P_temp] = KF.getPrediction()
data_pred = copy.deepcopy(data)
data_pred.position = pos[0:17]
import os
import cv2
import variables as vars
from KF import KalmanFilter
fourcc = cv2.VideoWriter_fourcc(*'XVID')
roi = None
face_cascade = cv2.CascadeClassifier('./haarcascade_frontalface_alt2.xml')
kalman = KalmanFilter()
def get_roi(event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
vars.start_pos = [x, y]
vars.select_done = False
elif event == cv2.EVENT_LBUTTONUP:
if vars.start_pos[0] == x and vars.start_pos[1] == y:
vars.select_done = False
else:
vars.end_pos = [x, y]
vars.select_done = True
if __name__ == "__main__":
file_name = "fuyou_1a.mp4"
file_name2 = "fuyou_1a.avi"
input_file = os.path.join("../origin_videos/unnormal/", file_name)
output_file = os.path.join("../processed_videos/unnormal/", file_name2)