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)