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], thresh_list[i][j])] x, gt = align_plots(x, gt, dist=0.8, use_totstat=True, align_idx=trigger_ind[1]) #rotate data ###Calculate ARMSE between estimate and Vicon armse_3d = compute_error(x[trigger_ind], gt, '3d') error_logger.update(armse_3d, trial_type, person, det_list[i], j)
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) visualize.plot_topdown([x_shoe, x_ared, gt]) plt.figure() plt.plot(data['zv_shoe_opt'][0]) ###Calculate ARMSE between estimate and Vicon shoe_error = compute_error(x_shoe, gt, '2d') ared_error = compute_error(x_ared, gt, '2d') print("ARMSE for ARED: {}".format(ared_error)) print("ARMSE for SHOE: {}".format(shoe_error))
best_detector = data['best_detector'][0] best_detector_count[best_detector] += 1 ###add custom detector and its zv output to lists: det_list = ['shoe'] #['shoe', 'ared', 'amvd', 'mbgtd', 'vicon'] zv_list = [ zv_shoe_opt, zv_ared_opt, zv_amvd_opt, zv_mbgtd_opt, zv_vicon_opt ] ins = INS(imu, sigma_a=0.00098, sigma_w=8.7266463e-5, T=1.0 / 200) #microstrain ###Estimate trajectory for i in range(0, len(det_list)): if load_traj != False: x = ins.baseline(zv=zv_list[i]) print(x.shape) x, gt = align_plots(x, gt) #rotate data saved_trajectories['{}_{}'.format(trial_name, det_list[i])] = x else: x = stored_trajectories['{}_{}'.format(trial_name, det_list[i])] ###Calculate ARMSE between estimate and Vicon armse_2d = compute_error(x, gt, '2d') armse_3d = compute_error(x, gt, '3d') # print("ARMSE for {}: {}".format(det_list[i], armse_2d)) trial_stats.append(armse_2d) trial_stats.append(armse_3d) stats.append(trial_stats)
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') elif (detector == "lstm"): zv_lstm = ins.Localizer.LSTM() x_out = ins.baseline(zv=zv_lstm) else: print("Invalid detector") exit