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')