def load_dataset(csv_file, input_format="maplab_console"):
"""
Loads a dataset in one of the supported formats and returns a Nx8 matrix
with the trajectory from the dataset with the following columns:
timestamp [s], position x, y, z [m], quaternion x, y, z, w
Supported formats:
- end_to_end (same as internal end_to_end representation)
- maplab_console (output of csv_export command in maplab)
- rovioli (output of --export_estimated_poses_to_csv flag from ROVIOLI)
- euroc_ground_truth (euroc ground truth CSVs as can be downloaded with
the datasets)
- orbslam (output from running ORB-SLAM)
"""
assert os.path.isfile(csv_file), "File " + csv_file + " doesn't exist."
NANOSECONDS_TO_SECONDS = 1e-9
trajectory = np.zeros((0, 8))
if input_format.lower() == "end_to_end":
trajectory = np.genfromtxt(csv_file, delimiter=',', skip_header=0)
elif input_format.lower() == "maplab_console":
trajectory = np.genfromtxt(csv_file, delimiter=',', skip_header=1)
trajectory = trajectory[:, 1:9]
trajectory[:, 0] *= NANOSECONDS_TO_SECONDS
elif input_format.lower() == "rovioli":
# Compute T_G_I from T_G_M and T_G_I.
estimator_data_raw = np.genfromtxt(csv_file,
delimiter=',',
skip_header=1)
num_elements = estimator_data_raw.shape[0]
trajectory = np.zeros((num_elements, 8))
for i in range(num_elements):
trajectory[i, 0] = estimator_data_raw[i, 0]
T_G_M = csv_row_data_to_transformation(estimator_data_raw[i, 1:4],
estimator_data_raw[i, 4:8])
T_M_I = csv_row_data_to_transformation(
estimator_data_raw[i, 8:11], estimator_data_raw[i, 11:15])
T_G_M = np.dot(T_G_M, T_M_I)
trajectory[i, 1:4] = T_G_M[0:3, 3]
q_G_I = Quaternion.from_rotation_matrix(T_G_M[0:3, 0:3])
trajectory[i, 4:8] = q_G_I.q
elif input_format.lower() == "euroc_ground_truth":
trajectory = np.genfromtxt(csv_file, delimiter=',', skip_header=1)
# We need to bring the Euroc ground truth data into the format required
# by hand-eye calibration.
trajectory[:, 0] *= NANOSECONDS_TO_SECONDS
# Reorder quaternion wxyz -> xyzw.
trajectory[:, [4, 5, 6, 7]] = trajectory[:, [5, 6, 7, 4]]
# Remove remaining columns.
trajectory = trajectory[:, 0:8]
elif input_format.lower() == "orbslam":
trajectory = np.genfromtxt(csv_file, delimiter=' ', skip_header=0)
# TODO(eggerk): Check quaternion convention from ORBSLAM.
else:
print "Unknown estimator input format."
print "Valid options are maplab_console, rovioli, orbslam."
assert False
return trajectory
def from_transformation_matrix(cls, transformation_matrix):
q_rot = Quaternion.from_rotation_matrix(transformation_matrix[0:3,
0:3])
pose_vec = np.zeros(7)
pose_vec[3:7] = q_rot.q
pose_vec[0:3] = transformation_matrix[0:3, 3]
return cls.from_pose_vector(pose_vec)
def align_datasets(estimator_data_unaligned_G_I,
ground_truth_data_unaligned_W_M,
estimate_scale,
align_data_to_use_meters=-1,
time_offset=0.0,
marker_calibration_file=None):
"""Sample and align the estimator trajectory to the ground truth trajectory.
Input:
- estimator_data_unaligned_G_I: Unaligned estimator trajectory expressed
from global frame G to IMU frame I.
- ground_truth_data_unaligned_W_M: Unaligned ground truth data expressed
from ground truth world frame W (e.g. Vicon frame) to marker frame M
(e.g. Vicon marker). M and I should be at the same position at a
given time.
- estimate_scale: Estimate a scale factor in addition to the transform
and apply it to the aligned estimator trajectory.
- align_data_to_use_meters: Only use data points up to a total data length
as given by this parameter. If a negative value is given, the
entire trajectory is used for the alignment.
- time_offset: Indicates the time offset (in seconds) between the data
from the bag file and the ground truth data. In the alignment, the
estimator data will be shifted by this offset.
- marker_calibration_file: Contains the calibration between the marker
frame M and the eye frame I. If no file is given, the frames M and I
are assumed to be identical.
Returns:
Sampled, aligned and transformed estimator trajectory, sampled ground
truth.
"""
if marker_calibration_file:
estimator_data_unaligned_G_I = apply_hand_eye_calibration(
estimator_data_unaligned_G_I, marker_calibration_file)
else:
estimator_data_unaligned_G_I = estimator_data_unaligned_G_I
[estimator_data_G_I, ground_truth_data_W_M
] = time_alignment.compute_aligned_poses(estimator_data_unaligned_G_I,
ground_truth_data_unaligned_W_M,
time_offset)
# Align trajectories (umeyama).
if align_data_to_use_meters < 0.0:
umeyama_estimator_data_G_I = estimator_data_G_I[:, 1:4]
umeyama_ground_truth_W_M = ground_truth_data_W_M[:, 1:4]
else:
trajectory_length = get_cumulative_trajectory_length_for_each_point(
ground_truth_data_W_M[:, 1:4])
align_num_data_points = bisect.bisect_left(trajectory_length,
align_data_to_use_meters)
umeyama_estimator_data_G_I = estimator_data_G_I[:align_num_data_points,
1:4]
umeyama_ground_truth_W_M = \
ground_truth_data_W_M[:align_num_data_points, 1:4]
T_W_G, scale = umeyama(umeyama_estimator_data_G_I.T,
umeyama_ground_truth_W_M.T, estimate_scale)
assert T_W_G.shape[0] == 4
assert T_W_G.shape[1] == 4
if estimate_scale:
assert scale > 0.0
print("Estimator scale:", 1. / scale)
estimator_data_G_I_scaled = estimator_data_G_I.copy()
estimator_data_G_I_scaled[:, 1:4] *= scale
ground_truth_data_G_M_aligned = ground_truth_data_W_M.copy()
num_data_points = ground_truth_data_W_M.shape[0]
for index in range(num_data_points):
# Get transformation matrix from quaternion (normalized) and position.
T_ground_truth_W_M = csv_row_data_to_transformation(
ground_truth_data_W_M[index,
1:4], ground_truth_data_W_M[index, 4:8] /
np.linalg.norm(ground_truth_data_W_M[index, 4:8]))
# Apply found calibration between G and M frame.
T_ground_truth_G_M = np.dot(np.linalg.inv(T_W_G), T_ground_truth_W_M)
# Extract quaternion and position.
q_ground_truth_G_M = Quaternion.from_rotation_matrix(
T_ground_truth_G_M[0:3, 0:3])
ground_truth_data_G_M_aligned[index, 4:8] = q_ground_truth_G_M.q
ground_truth_data_G_M_aligned[index, 1:4] = T_ground_truth_G_M[0:3, 3]
return estimator_data_G_I_scaled, ground_truth_data_G_M_aligned
def align_datasets(estimator_data_G_I_path,
ground_truth_data_W_M_path,
estimator_input_format="rovioli"):
"""
Sample and align the ground truth trajectory to the estimator trajectory.
Input frames:
Estimator: G (Global, from rovioli) --> I (imu, from rovioli)
Ground truth: W (Ground truth world, e.g., Vicon frame)
--> M (ground truth marker, e.g., Vicon marker)
(M and I should be at the same position at a given time.)
Returns: aligned estimator trajectory, aligned and sampled ground truth
trajectory.
Output data frames:
Estimator: G --> I
Ground truth: G --> M
(M and I should be at the same position.)
"""
assert os.path.isfile(estimator_data_G_I_path)
assert os.path.isfile(ground_truth_data_W_M_path)
estimator_data_unaligned_G_I = np.zeros((0, 8))
if estimator_input_format.lower() == "maplab_console":
estimator_data_unaligned_G_I = np.genfromtxt(estimator_data_G_I_path,
delimiter=',',
skip_header=1)
estimator_data_unaligned_G_I = estimator_data_unaligned_G_I[:, 1:9]
# Convert timestamp ns -> s.
estimator_data_unaligned_G_I[:, 0] *= 1e-9
elif estimator_input_format.lower() == "rovioli":
# Compute T_G_I from T_G_M and T_G_I
estimator_data_raw = np.genfromtxt(estimator_data_G_I_path,
delimiter=',',
skip_header=1)
num_elements = estimator_data_raw.shape[0]
estimator_data_unaligned_G_I = np.zeros((num_elements, 8))
for i in range(num_elements):
estimator_data_unaligned_G_I[i, 0] = estimator_data_raw[i, 0]
T_G_M = csv_row_data_to_transformation(estimator_data_raw[i, 1:4],
estimator_data_raw[i, 4:8])
T_M_I = csv_row_data_to_transformation(
estimator_data_raw[i, 8:11], estimator_data_raw[i, 11:15])
T_G_M = np.dot(T_G_M, T_M_I)
estimator_data_unaligned_G_I[i, 1:4] = T_G_M[0:3, 3]
q_G_I = Quaternion.from_rotation_matrix(T_G_M[0:3, 0:3])
estimator_data_unaligned_G_I[i, 4:8] = q_G_I.q
elif estimator_input_format.lower() == "orbslam":
estimator_data_unaligned_G_I = np.genfromtxt(estimator_data_G_I_path,
delimiter=' ',
skip_header=0)
# TODO(eggerk): Check quaternion convention from ORBSLAM.
else:
print "Unknown estimator input format."
print "Valid options are maplab_console, rovioli, orbslam."
assert False
ground_truth_data_unaligned_W_M = np.genfromtxt(ground_truth_data_W_M_path,
delimiter=',',
skip_header=1)
ground_truth_data_unaligned_W_M = \
convert_ground_truth_data_into_target_format(
ground_truth_data_unaligned_W_M)
# This is needed for the alignment and sampling. It indicates the time offset
# between the data from Rovioli/from the bag file and the ground truth data.
# In the case of the Euroc datasets, this is 0 as they image/imu data and
# ground truth is already synchronized.
time_offset = 0
[ground_truth_data_W_M, estimator_data_G_I] = \
time_alignment.compute_aligned_poses(
ground_truth_data_unaligned_W_M, estimator_data_unaligned_G_I,
time_offset)
# Create a matrix of the right size to store aligned trajectory.
ground_truth_data_aligned_G_M = ground_truth_data_W_M
# Align trajectories (umeyama).
R_G_W, t_G_W, scale = umeyama(ground_truth_data_W_M[:, 1:4].T,
estimator_data_G_I[:, 1:4].T)
T_G_W = np.identity(4)
T_G_W[0:3, 0:3] = R_G_W
T_G_W[0:3, 3] = t_G_W
if abs(1 - scale) < 1e-8:
print "WARNING: Scale is not 1, but", scale, "\b."
num_data_points = estimator_data_G_I.shape[0]
for index in range(num_data_points):
# Create transformations to compare.
T_ground_truth_G_M = np.dot(
T_G_W,
csv_row_data_to_transformation(
ground_truth_data_W_M[index,
1:4], ground_truth_data_W_M[index, 4:8] /
np.linalg.norm(ground_truth_data_W_M[index, 4:8])))
ground_truth_data_aligned_G_M[index, 1:4] = T_ground_truth_G_M[0:3, 3]
q_ground_truth_G_M = Quaternion.from_rotation_matrix(
T_ground_truth_G_M[0:3, 0:3])
ground_truth_data_aligned_G_M[index, 4:8] = q_ground_truth_G_M.q
return estimator_data_G_I, ground_truth_data_aligned_G_M
def align_datasets(
estimator_data_G_I_path,
ground_truth_data_W_M_path,
estimator_csv_data_from_console=False):
"""
Sample and align the ground truth trajectory to the estimator trajectory.
estimator_csv_data_from_console: If set to true, the data from
estimator_data_path is in the format as given
by the CSV export of the maplab console.
Otherwise, the CSV file is expected to come
from Rovioli.
Input frames:
Estimator: G (Global, from rovioli) --> I (imu, from rovioli)
Ground truth: W (Ground truth world, e.g., Vicon frame)
--> M (ground truth marker, e.g., Vicon marker)
(M and I should be at the same position at a given time.)
Returns: aligned estimator trajectory, aligned and sampled ground truth
trajectory.
Output data frames:
Estimator: G --> I
Ground truth: G --> M
(M and I should be at the same position.)
"""
assert os.path.isfile(estimator_data_G_I_path)
assert os.path.isfile(ground_truth_data_W_M_path)
estimator_data_unaligned_G_I = np.zeros((0, 8))
if estimator_csv_data_from_console:
estimator_data_unaligned_G_I = np.genfromtxt(
estimator_data_G_I_path, delimiter=',', skip_header=1)
estimator_data_unaligned_G_I = estimator_data_unaligned_G_I[:, 1:9]
# Convert timestamp ns -> s.
estimator_data_unaligned_G_I[:, 0] *= 1e-9
else:
# Compute T_G_I from T_G_M and T_G_I
estimator_data_raw = np.genfromtxt(
estimator_data_G_I_path, delimiter=',', skip_header=1)
num_elements = estimator_data_raw.shape[0]
estimator_data_unaligned_G_I = np.zeros((num_elements, 8))
for i in range(num_elements):
estimator_data_unaligned_G_I[i, 0] = estimator_data_raw[i, 0]
T_G_M = csv_row_data_to_transformation(
estimator_data_raw[i, 1:4], estimator_data_raw[i, 4:8])
T_M_I = csv_row_data_to_transformation(
estimator_data_raw[i, 8:11], estimator_data_raw[i, 11:15])
T_G_M = np.dot(T_G_M, T_M_I)
estimator_data_unaligned_G_I[i, 1:4] = T_G_M[0:3, 3]
q_G_I = Quaternion.from_rotation_matrix(T_G_M[0:3, 0:3])
estimator_data_unaligned_G_I[i, 4:8] = q_G_I.q
ground_truth_data_unaligned_W_M = np.genfromtxt(
ground_truth_data_W_M_path, delimiter=',', skip_header=1)
ground_truth_data_unaligned_W_M = \
convert_ground_truth_data_into_target_format(
ground_truth_data_unaligned_W_M)
# This is needed for the alignment and sampling. It indicates the time offset
# between the data from Rovioli/from the bag file and the ground truth data.
# In the case of the Euroc datasets, this is 0 as they image/imu data and
# ground truth is already synchronized.
time_offset = 0
[ground_truth_data_W_M, estimator_data_G_I] = \
time_alignment.compute_aligned_poses(
ground_truth_data_unaligned_W_M, estimator_data_unaligned_G_I,
time_offset)
# Create a matrix of the right size to store aligned trajectory.
ground_truth_data_aligned_G_M = ground_truth_data_W_M
# Align trajectories (umeyama).
R_G_W, t_G_W, c = umeyama(
ground_truth_data_W_M[:, 1:4].T, estimator_data_G_I[:, 1:4].T)
T_G_W = np.identity(4)
T_G_W[0:3, 0:3] = R_G_W
T_G_W[0:3, 3] = t_G_W
num_data_points = estimator_data_G_I.shape[0]
for index in range(num_data_points):
# Create transformations to compare.
T_ground_truth_G_M = np.dot(T_G_W, csv_row_data_to_transformation(
ground_truth_data_W_M[index, 1:4],
ground_truth_data_W_M[index, 4:8] /
np.linalg.norm(ground_truth_data_W_M[index, 4:8])))
ground_truth_data_aligned_G_M[index, 1:4] = T_ground_truth_G_M[0:3, 3]
q_ground_truth_G_M = Quaternion.from_rotation_matrix(
T_ground_truth_G_M[0:3, 0:3])
ground_truth_data_aligned_G_M[index, 4:8] = q_ground_truth_G_M.q
return estimator_data_G_I, ground_truth_data_aligned_G_M
def test_quaternion_from_rotation_matrix(self):
rotation_matrix = np.array([[1, 0, 0], [0, 0, 1], [0, -1, 0]])
q = Quaternion.from_rotation_matrix(rotation_matrix)
expected_quaternion = Quaternion(-np.sqrt(2) / 2, 0, 0, np.sqrt(2) / 2)
npt.assert_allclose(q.q, expected_quaternion.q)
