def generate_se3_from_groundtruth(groundtruth_list):
tx = float(groundtruth_list[0])
ty = float(groundtruth_list[1])
tz = float(groundtruth_list[2])
qx = float(groundtruth_list[3])
qy = float(groundtruth_list[4])
qz = float(groundtruth_list[5])
qw = float(groundtruth_list[6])
#qx *= -1
#qy *= -1
#qz *= -1
#qw *= -1
se3 = np.identity(4)
roll, pitch, yaw = SE3.Quaternion_toEulerianRadians(qx, qy, qz, qw)
#roll*=-1
#pitch*=-1
#yaw*=-1
SO3 = SE3.makeS03(roll, pitch, yaw) # seems to be more precise
#SO3 = SE3.quaternion_to_s03(qx,qy,qz,qw)
se3[0:3, 0:3] = SO3[0:3, 0:3]
se3[0, 3] = tx
se3[1, 3] = ty
se3[2, 3] = tz
return se3
match_text = dataset_root + 'matches.txt'
groundtruth_text = dataset_root + 'groundtruth.txt'
groundtruth_dict = associate.read_file_list(groundtruth_text)
rgb_folder = dataset_root + rgb_folder
depth_folder = dataset_root + depth_folder
rgb_files = ListGenerator.get_files_from_directory(rgb_folder, delimiter='.')
depth_files = ListGenerator.get_files_from_directory(depth_folder,
delimiter='.')
rgb_file_total = len(rgb_files)
depth_file_total = len(depth_files)
euler = SE3.Quaternion_toEulerianRadians(0.8772, -0.1170, 0.0666, -0.4608)
ground_truth_acc = np.identity(4, Utils.matrix_data_type)
se3_estimate_acc = np.identity(4, Utils.matrix_data_type)
ground_truth_list = []
pose_estimate_list = []
ref_image_list = []
target_image_list = []
vo_twist_list = []
depth_factor = 5000.0
#depth_factor = 1.0
use_ndc = True
calc_vo = True
plot_steering = True