def __init__(self, id, bbox, centroid, area, tracker_type, frame=None):
self.id = id
self.centroid = centroid
self.history_centroid = [centroid]
self.history_centroid_predicted = [centroid] # just for initialize
self.bbox = bbox # Define an initial bounding box, bbox = (287, 23, 86, 320)
self.area = [area]
self.totalVisible = 0
self.consecutiveInvisible = 0
# speed computation parameters
self.centroid_memory = centroid
self.speed = 0
self.history_speed = []
self.visible = True
self.age = 1
self.score = 0
self.tracker_type = tracker_type
#filter
# TODO: Check tracker creation
if self.tracker_type == 'kalman filter':
self.tracker = kalman_filter(initialPosition=centroid)
else:
self.tracker = kcf(frame, bbox, tracker_type)
lzs.append(lsensor.measure(target))
rz = rsensor.measure(target)
rzs_polar.append(rz)
rho = rz[0][0]
phi = rz[1][0]
rhodot = rz[2][0]
rzs_cartesian.append(polar_to_cartesian(rho, phi, rhodot))
true_x.append(target.x)
true_y.append(target.y)
update_fusion_x = []
update_fusion_y = []
measurement_x = []
measurement_y = []
kf = kalman_filter(F, H_laser, P, R_laser, dt)
ekf = extended_kalman_filter(F, P, R_radar, dt)
def fusion_process(x, iter, lzs, rzs_polar, P):
for n in range(iter):
if n % 2 == 0:
# measurement update
z = lzs[n]
measurement_x.append(z[0][0])
measurement_y.append(z[1][0])
x, P = kf.fusion(x, P, z)
# store update data
from opticalFlow import * >>>>>>> b6c635a... Almost there import matplotlib.pyplot as plt vid1 = 'CIS581Project4PartCDatasets/Easy/FrankUnderwood.mp4' vid2 = 'CIS581Project4PartCDatasets/Easy/MrRobot.mp4' NUM_FEATURES = 68 WEIGHT_DLIB = 0.05 output_im = [] output_im_ = [] frames1, frames2 = getFrames(vid1,vid2) length = len(frames2) kalman = kalman_filter(NUM_FEATURES) kalman2 = kalman_filter(NUM_FEATURES) for i in range(0, length): im1, landmarks1 = read_im_and_landmarks(frames1[i]) im2, landmarks2 = read_im_and_landmarks(frames2[i]) if i == 0: kalman.statePost = np.vstack((np.reshape(np.float32(landmarks1),(NUM_FEATURES*2,1)),np.zeros((NUM_FEATURES*2,1),dtype=np.float32))) kalman2.statePost = np.vstack((np.reshape(np.float32(landmarks2),(NUM_FEATURES*2,1)),np.zeros((NUM_FEATURES*2,1),dtype=np.float32))) # print kalman.statePost.shape if i >= 1: landmarks1_optical_flow, st1, err = optical_flow(prev_frame_1, frames1[i], landmarks1) landmarks2_optical_flow, st2, err = optical_flow(prev_frame_2, frames2[i], landmarks2)
reader = csv_time_reader()
for i in range(1,len(sys.argv)):
arg = sys.argv[i]
fd = open(arg, 'rb')
arg_first_dot = arg.find(".csv")
arg_last_underscore = arg.rfind("_") + 1
name = arg[arg_last_underscore:arg_first_dot]
reader.add_file(name, fd)
t0 = reader.get_time(reader.get_min_time_name())
kalman = kalman_filter()
while reader.get_min_time_name():
name = reader.get_min_time_name()
time = reader.get_time(name)
row = reader.get_row_next(name)
process(name, time, row)
if int(time) % 600 == 0:
sys.stderr.write(".")
sys.stderr.flush()
tgs_x = kalman.get_true_groundspeed_x().predict(time, 0.0)
tgs_y = kalman.get_true_groundspeed_y().predict(time, 0.0)
vario = kalman.get_vario().predict(time, 0.0)
height = kalman.get_height().predict(time, vario)
wind_x = kalman.get_wind_x().predict(time, 0.0)