pose_estimate_list_2.append(se3_estimate_acc_2)
if data_file_3:
SE3_est_3 = pose_estimate_list_loaded_3[i]
se3_estimate_acc_3 = np.matmul(se3_estimate_acc_3, SE3_est_3)
pose_estimate_list_3.append(se3_estimate_acc_3)
if data_file_4:
SE3_est_4 = pose_estimate_list_loaded_4[i]
se3_estimate_acc_4 = np.matmul(se3_estimate_acc_4, SE3_est_4)
pose_estimate_list_4.append(se3_estimate_acc_4)
delta = 30
if (count - 1) - start_count >= delta:
print("1: " +
str(SE3.rmse_avg_raw(ground_truth_list, pose_estimate_list, delta)))
if data_file_2:
print("2: " + str(
SE3.rmse_avg_raw(ground_truth_list, pose_estimate_list_2, delta)))
if data_file_3:
print("3: " + str(
SE3.rmse_avg_raw(ground_truth_list, pose_estimate_list_3, delta)))
if data_file_4:
print("4: " + str(
SE3.rmse_avg_raw(ground_truth_list, pose_estimate_list_4, delta)))
handles = []
visualizer = Visualizer.Visualizer(ground_truth_list,
plot_steering=False,
plot_trajectory=False,
se3_estimate_acc_2 = np.matmul(se3_estimate_acc_2, SE3_est_2)
pose_estimate_list_2.append(se3_estimate_acc_2)
if data_file_3:
SE3_est_3 = pose_estimate_list_loaded_3[i]
se3_estimate_acc_3 = np.matmul(se3_estimate_acc_3, SE3_est_3)
pose_estimate_list_3.append(se3_estimate_acc_3)
if data_file_4:
SE3_est_4 = pose_estimate_list_loaded_4[i]
se3_estimate_acc_4 = np.matmul(se3_estimate_acc_4, SE3_est_4)
pose_estimate_list_4.append(se3_estimate_acc_4)
delta = 30
if (count - 1) - start_count >= delta:
print(SE3.rmse_avg_raw(ground_truth_list,pose_estimate_list, delta))
visualizer = Visualizer.Visualizer(ground_truth_list,plot_steering=False, plot_trajectory=False)
visualizer.visualize_ground_truth(clear=True,draw=False)
if plot_vo:
visualizer.visualize_poses(pose_estimate_list, draw= False)
if data_file_2:
visualizer.visualize_poses(pose_estimate_list_2, draw= False, style='-ro')
if data_file_3:
visualizer.visualize_poses(pose_estimate_list_3, draw= False, style='-bx')
if data_file_3:
visualizer.visualize_poses(pose_estimate_list_4, draw= False, style='-bo')
print('visualizing..')
visualizer.show()