def main():
print("Optimizing thresholds for all VICON data")
print("Warning: This may take several hours (please download the processed data instead of running this)")
source_dir='data/vicon/raw/'
for folder in sorted(glob.glob('{}/*'.format(source_dir))):
print('Importing file: ' + folder)
data = sio.loadmat('{}/processed_data.mat'.format(folder))
imu = data['imu']
gt = data['gt']
gt_rpy = data['gt_rpy']
ts = data['ts']
ins = INS(imu, sigma_a = 0.00098, sigma_w = 8.7266463e-5, T=1.0/200)
ins.Localizer.ts = ts
ins.Localizer.gt = gt
G_vicon_opt, vicon_err, zv_vicon_opt = optimize_gamma(ins, gt, thresh=list(np.arange(1e-2,1e-1,0.25e-2))+list(np.arange(1e-1,1,0.25e-1)), detector='vicon') #do not call temporal_align prior to this step
G_shoe_opt, shoe_err, zv_shoe_opt = optimize_gamma(ins, gt, W=5, thresh=list(np.arange(1e5,1e6,0.25e5))+list(np.arange(1e6,1e7,0.25e6))+list(np.arange(1e7,1e8,0.25e7))+list(np.arange(1e8,1e9,0.25e8)), detector='shoe')
G_amvd_opt, amvd_err, zv_amvd_opt = optimize_gamma(ins, gt, W=5, thresh=list(np.arange(1e-4,1e-3,0.25e-4))+list(np.arange(1e-3,1e-2,0.25e-2))+list(np.arange(1e-2,1e-1,0.25e-2))+list(np.arange(1e-1,2,0.25e-1)), detector='amvd')
G_ared_opt, ared_err, zv_ared_opt = optimize_gamma(ins, gt, W=5, thresh=list(np.arange(1e-2,1e-1,0.25e-2))+list(np.arange(1e-1,1,0.25e-1))+list(np.arange(1,3, 0.05)), detector='ared')
G_mbgtd_opt, mbgtd_err, zv_mbgtd_opt = optimize_gamma(ins, gt, W=2, thresh=list(np.arange(1e-3,1e-2,0.25e-3))+list(np.arange(1e-2,1e-1,0.25e-2))+list(np.arange(1e-1,1,0.25e-1)), detector='mbgtd')
data['G_vicon_opt'] = G_vicon_opt
data['zv_vicon_opt'] = zv_vicon_opt
data['G_shoe_opt'] = G_shoe_opt
data['zv_shoe_opt'] = zv_shoe_opt
data['G_ared_opt'] = G_ared_opt
data['zv_ared_opt'] = zv_ared_opt
data['G_amvd_opt'] = G_amvd_opt
data['zv_amvd_opt'] = zv_amvd_opt
data['G_mbgtd_opt'] = G_mbgtd_opt
data['zv_mbgtd_opt'] = zv_mbgtd_opt
det = ['shoe', 'ared', 'amvd', 'mbgtd', 'vicon']
errors = np.array([shoe_err, ared_err, ared_err, mbgtd_err, vicon_err])
best_idx = np.argmin(errors)
best_error = errors[best_idx]
best_det = det[best_idx]
data['trial_error'] = errors
data['best_error'] = best_error
data['best_detector'] = best_det
output_name = folder.replace(source_dir,'')
print("Processed folder {}".format(output_name))
sio.savemat('data/vicon/processed/{}.mat'.format(output_name), data)
load_traj = True #set to false to recompute the trajectories, or true to reload the previously saves trajectories (much faster to reload)
if load_traj == True:
stored_trajectories = sio.loadmat(
"results/stored_hallway_trajectories.mat")
for f in sorted(glob.glob('{}*/*/*/*.mat'.format(source_dir))):
trial_name = f.replace(source_dir, '').replace('/processed_data.mat', '')
print(trial_name)
trial_type, person, folder = trial_name.split('/')
trial_stats = [trial_type, person, folder]
data = sio.loadmat(f)
imu = data['imu']
ts = data['ts'][0]
gt = data['gt']
trigger_ind = data['gt_idx'][0]
ins = INS(imu, sigma_a=0.00098, sigma_w=8.7266463e-5,
T=1.0 / 200) #microstrain
for i in range(0, len(det_list)): #Iterate through detector list
for j in range(0,
len(thresh_list[i])): #iterate through threshold list
if load_traj != True:
zv = ins.Localizer.compute_zv_lrt(W=W_list[i],
G=thresh_list[i][j],
detector=det_list[i])
x = ins.baseline(zv=zv)
saved_trajectories["{}_{}_{}_det_{}_G_{}".format(
trial_type, person, folder, det_list[i],
thresh_list[i][j])] = x
else:
x = stored_trajectories["{}_{}_{}_det_{}_G_{}".format(
trial_type, person, folder, det_list[i],
stair_demo = True #processes a trajectory from our stair dataset
hallway_demo = True #processes a trajectory from our hallway dataset
adaptive_demo = True #runs our adaptive zero-velocity detector with motion classification
lstm_demo = True #runs our zero-velocity classifier
if vicon_demo:
print("Vicon Demo")
source_dir = "data/vicon/processed/"
folder = "2017-11-27-11-13-10"
data = sio.loadmat('{}{}.mat'.format(source_dir, folder))
imu = data['imu']
ts = data['ts'][0]
gt = data['gt']
ins = INS(imu, sigma_a=0.00098, sigma_w=8.7266463e-5, T=1.0 / 200)
###Optimize zero-velocity threshold for given trial
#G_opt_shoe, _, zv_opt_shoe = optimize_gamma(ins, gt, thresh=[0.5e7, 10e7], W=5, detector='shoe')
#G_opt_ared, _, zv_opt_shoe = optimize_gamma(ins, gt, thresh=[0.1, 2], W=5, detector='ared')
###load the pre-computed optimal thresholds
G_opt_shoe = float(data['G_shoe_opt'])
G_opt_ared = float(data['G_ared_opt'])
###Estimate trajectory
x_shoe = ins.baseline(W=5, G=G_opt_shoe, detector='shoe')
x_ared = ins.baseline(W=5, G=G_opt_ared, detector='ared')
x_shoe, _ = align_plots(x_shoe, gt) #rotate data
x_ared, _ = align_plots(x_ared, gt)
"field.angular_velocity.y", "field.angular_velocity.z"
]
dataTypes = [int, float, float, float, float, float, float]
status, userData = readROSBagCSV(data_dir + fileName + '.csv',
fields=fieldNames,
dtype=dataTypes)
ros_data = userData.view((float, len(userData.dtype.names)))
print("ROS Bag Demo: " + fileName)
imu = ros_data[:, 1:]
timeStep = 1.0 / 40
print("Input shape: ", imu.shape)
ins = INS(imu, sigma_a=0.00098, sigma_w=8.7266463e-5, T=timeStep)
detector = "shoe"
if (detector == "shoe"):
# load the pre-computed optimal thresholds
G_opt_shoe = 2.5e8
#Estimate trajectory
x_out = ins.baseline(W=5, G=G_opt_shoe, detector='shoe')
elif (detector == "ared"):
# load the pre-computed optimal thresholds
G_opt_ared = 1.5000000000000004
#Estimate trajectory
x_out = ins.baseline(W=5, G=G_opt_ared, detector='ared')