def test_check_update_in_update(self):
x = 0.2
v = 2.3
kf = KF(initial_x=x, initial_v=v, accel_variance=1.2)
kf.update(meas_value=0.1, meas_variance=0.01)
self.assertNotEqual(kf.pos, x)
self.assertAlmostEqual(kf.vel, v)
def test_calling_update_does_not_crash(self):
x = 0.2
v = 0.8
kf = KF(x, v, accel_variance=1.2)
meas_value = 0.1
meas_variance = 0.1
kf.update(meas_value, meas_variance)
def test_calling_update_decreases_state_uncertainty(self):
x = 0.2
v = 2.3
kf = KF(x, v, 1.2)
det_before = np.linalg.det(kf.cov)
kf.update(0.1, 0.01)
det_after = np.linalg.det(kf.cov)
self.assertLess(det_after, det_before)
def test_calling_update_decreases_state_uncertainty(self):
x = 0.2
v = 2.3
kf = KF(initial_x=x, initial_v=v, accel_variance=1.2)
det_before = np.linalg.det(kf.cov)
kf.update(meas_value=0.1, meas_variance=0.01)
det_after = np.linalg.det(kf.cov)
self.assertLess(det_after, det_before)
def test_calling_update_decreases_state_uncertainty(self):
x = 0.2
v = 0.8
meas_value = 0.1
meas_variance = 0.1
kf = KF(x, v, accel_variance=1.2)
for i in range(10):
det_cov_before = np.linalg.det(kf.cov)
#print(det_cov_before)
#print("---")
kf.update(meas_value, meas_variance)
det_cov_after = np.linalg.det(kf.cov)
#print(det_cov_after)
#print(" ")
self.assertLess(det_cov_after, det_cov_before)
def test_update_ok(self):
x = 0.2
v = 2.3
kf = KF(initial_x=x, initial_v=v, accel_variance=1.2)
kf.update(meas_value=0.1, meas_variance=0.1)
mus=[]#creating a list for storing the mean
covs=[]#creating a list for storing the covariance
real_xs = []
real_vs = []
for step in range(NUM_STEPS):
covs.append(kf.cov)
mus.append(kf.mean)
real_x = real_x + DT * real_v
kf.predict(dt=DT)
if step != 0 and step%MEAS_EVERY_STEP == 0:
kf.update(meas_value = real_x + np.random.randn()*np.sqrt(meas_variance) , meas_variance = meas_variance)
real_xs.append(real_x)
real_vs.append(real_v)
"""
plt.subplot(2,1,1)
plt.title("Position")
plt.plot([mu[0] for mu in mus],'r')#'r'--> denotes the red color of the line
plt.plot(real_xs,'b')
#FOr creating disturbances
plt.plot([mu[0]-2*np.sqrt(cov[0,0])for mu,cov in zip(mus,covs)],'r--')#np.sqrt is used to find the square root of a list
#'r--" denotes the red color dotted line
plt.plot([mu[0]+2*np.sqrt(cov[0,0])for mu,cov in zip(mus,covs)],'r--')
"""
"""
The zip() function returns a zip object, which is an iterator of tuples where the first item in each passed iterator is paired together, and then the second item in each passed iterator are paired together etc.
def _track(net, meta, data, output_path):
first_frame = next(data)
init_states = {}
track_id = -1
# initialize tracker
detections = _detect_people(net, meta, first_frame)
for detection in detections:
track_id += 1
cx, cy, w, h = detection[2]
init_states[track_id] = np.array([cx, cy, w * h, w / h, 0, 0, 0])
filt = KF(init_states)
_output_tracks(filt.latest_live_states(), first_frame, output_path)
# process remaining frames
for frame in data:
# predict motion of BB for existing tracks
predictions = filt.predict()
bbs = list(map(lambda d: d[2], _detect_people(net, meta, frame)))
keys = list(predictions.keys())
iter_bounds = max(len(keys), len(bbs))
# Hungarian method for assignment
# first build cost matrix
cost_mat = np.zeros((iter_bounds, iter_bounds))
for i in range(min(iter_bounds, len(bbs))):
for j in range(min(iter_bounds, len(keys))):
cost_mat[i, j] = _iou(bbs[i],
_state_to_bb(predictions[keys[j]]))
# TODO put optimizer call in for loop condition
# then solve the optimization problem
rows, cols = optimize.linear_sum_assignment(cost_mat, maximize=True)
# assign detections to old or new tracks, as appropriate
# (r,c) indexes an IOU in cost_mat, r coresponds to a detection bb
# c is a track from the previous frame
assignments = {}
new_tracks = {}
for r, c in zip(rows, cols):
if r < len(bbs):
cx, cy, w, h = bbs[r]
else:
continue
state = np.array([cx, cy, w * h, w / h, 0, 0, 0])
if cost_mat[r, c] >= MIN_IOU: # new detection for existing track
assignments[keys[c]] = state
else: # new track
track_id += 1
new_tracks[track_id] = state
# build the measurements
#_output_tracks(assignments, frame, output_path, prefix='assignment')
ys = {}
for label, last_state in filt.latest_live_states().items():
if label in assignments:
astate = assignments[label]
ys[label] = np.array([
astate[0], astate[1], astate[2], astate[3],
astate[0] - last_state[0], astate[1] - last_state[1],
astate[2] - last_state[2]
])
else:
filt.kill_state(label)
filt.update(ys, predictions)
for label, state in new_tracks.items():
filt.birth_state(label, state)
_output_tracks(filt.latest_live_states(), frame, output_path)
mask = np.zeros((img.shape[0], img.shape[1])).astype(np.uint8)
pos = kf.pos.astype(np.int)
x = pos[0, 0]
y = pos[1, 0]
w = pos[2, 0]
h = pos[3, 0]
print(x, y, w, h)
rect = cv2.rectangle(prevImg, (x, y), (x + w, y + h), (0, 255, 0), 2)
imgPath = "img" + str(picNum) + ".jpg"
cv2.imwrite(imgPath, rect)
im2 = Image.open(imgPath)
frames.append(im2)
mask[y:y + h, x:x + w] = 1
list_kp1, list_kp2 = getMatch(prevImg, img, 2, prevMask, mask)
new_x = np.min(np.array([item[0] for item in list_kp2])) - 5
new_y = np.min(np.array([item[1] for item in list_kp2])) - 5
new_w = np.max(np.array([item[0] for item in list_kp2])) - new_x + 10
new_h = np.max(np.array([item[1] for item in list_kp2])) - new_y + 10
# if step != 0 and step % MEAS_EVERY_STEPS == 0:
kf.update(
meas_value=np.array(
[new_x, new_y, new_w,
new_h]).T, # real_x + np.random.randn() * np.sqrt(meas_variance),
meas_variance=meas_variance)
prevImg = img
prevMask = mask
im.save("Gif_test.gif", save_all=True, append_images=frames)
