def test_quaternion_to_transformation_matrix(self):
q = Quaternion(0.5, 0.5, 0.5, 0.5)
transformation_matrix = q.to_transformation_matrix()
expected_transformation_matrix = np.array([[0, 0, 1, 0], [1, 0, 0, 0],
[0, 1, 0, 0], [0, 0, 0, 1]])
npt.assert_allclose(transformation_matrix,
expected_transformation_matrix)
def test_multiplication_with_scalar(self):
qr = Quaternion(0.5, 0.5, -0.5, 0.5)
qt = Quaternion(1, 3, 3, 0)
dq = DualQuaternion(qr, qt)
dq_expected = np.array([1.25, 1.25, -1.25, 1.25, 2.5, 7.5, 7.5, 0]).T
# Note, the scaling only applies to the translational part.
npt.assert_allclose((dq * 2.5).dq, dq_expected)
def compute_orientation_mean_and_rmse_absolute(q_AB_list, q_AC_list):
assert q_AB_list.shape == q_AC_list.shape
assert q_AC_list.shape[1] == 4
q_AB_0 = Quaternion(q_AB_list[0, 0], q_AB_list[0, 1], q_AB_list[0, 2],
q_AB_list[0, 3])
q_AC_0 = Quaternion(q_AC_list[0, 0], q_AC_list[0, 1], q_AC_list[0, 2],
q_AC_list[0, 3])
orientation_errors_rad = np.zeros(q_AB_list.shape[0])
for idx in range(q_AB_list.shape[0]):
q_AB_xyzw = q_AB_list[idx, :]
q_AC_xyzw = q_AC_list[idx, :]
q_AB = Quaternion(q_AB_xyzw[0], q_AB_xyzw[1], q_AB_xyzw[2],
q_AB_xyzw[3])
q_AC = Quaternion(q_AC_xyzw[0], q_AC_xyzw[1], q_AC_xyzw[2],
q_AC_xyzw[3])
# Zero the (arbitary) initial poses.
q_AB = q_AB_0.inverse() * q_AB
q_AC = q_AC_0.inverse() * q_AC
q_BC = q_AB.inverse() * q_AC
angle_BC = q_BC.angle_axis()[3]
if angle_BC > np.pi:
angle_BC -= 2 * np.pi
assert q_AC_list.shape[1] == 4
assert angle_BC <= np.pi
assert angle_BC >= -np.pi
orientation_errors_rad[idx] = np.abs(angle_BC)
return ErrorStatistics(orientation_errors_rad)
def read_time_stamped_poses_from_csv_file(csv_file,
JPL_quaternion_format=False):
"""
Reads time stamped poses from a CSV file.
Assumes the following line format:
timestamp [s], x [m], y [m], z [m], qx, qy, qz, qw
The quaternion is expected in Hamilton format, if JPL_quaternion_format is True
it expects JPL quaternions and they will be converted to Hamiltonian quaternions.
"""
with open(csv_file, 'r') as csvfile:
csv_reader = csv.reader(csvfile, delimiter=',', quotechar='|')
time_stamped_poses = np.array(list(csv_reader))
time_stamped_poses = time_stamped_poses.astype(float)
# Extract the quaternions from the poses.
times = time_stamped_poses[:, 0].copy()
poses = time_stamped_poses[:, 1:]
quaternions = []
for pose in poses:
pose[3:] /= np.linalg.norm(pose[3:])
if JPL_quaternion_format:
quaternion_JPL = np.array([-pose[3], -pose[4], -pose[5], pose[6]])
quaternions.append(Quaternion(q=quaternion_JPL))
else:
quaternions.append(Quaternion(q=pose[3:]))
return (time_stamped_poses.copy(), times, quaternions)
def test_scalar(self):
qr = Quaternion(1, 2, 3, 4)
qt = Quaternion(1, 3, 3, 1)
dq = DualQuaternion(qr, qt)
scalar = dq.scalar()
scalar_expected = np.array([0., 0., 0., 4., 0., 0., 0., 1.]).T
npt.assert_allclose(scalar.dq, scalar_expected, atol=1e-6)
def from_vector(cls, dq):
dual_quaternion_vector = None
if isinstance(dq, np.ndarray):
dual_quaternion_vector = dq.copy()
else:
dual_quaternion_vector = np.array(dq)
return cls(Quaternion(q=dual_quaternion_vector[0:4]),
Quaternion(q=dual_quaternion_vector[4:8]))
def test_division(self):
qr_1 = Quaternion(1, 2, 3, 4)
qt_1 = Quaternion(1, 3, 3, 6)
dq_1 = DualQuaternion(qr_1, qt_1)
identity_dq = np.array([0., 0., 0., 1., 0., 0., 0., 0.]).T
npt.assert_allclose((dq_1 / dq_1).dq, identity_dq, atol=1e-6)
# TODO(ff): Fix this test.
qr_2 = Quaternion(1, 4, 5, 1)
qt_2 = Quaternion(-4, 2, 3, 4)
dq_2 = DualQuaternion(qr_2, qt_2)
def test_addition(self):
qr_1 = Quaternion(1., 2., 3., 4.)
qt_1 = Quaternion(1., 3., 3., 0.)
dq_1 = DualQuaternion(qr_1, qt_1)
qr_2 = Quaternion(3., 5., 3., 2.)
qt_2 = Quaternion(-4., 2., 3., 0.)
dq_2 = DualQuaternion(qr_2, qt_2)
dq_expected = np.array([4., 7., 6., 6., -3., 5., 6., 0.]).T
npt.assert_allclose((dq_1 + dq_2).dq, dq_expected, rtol=1e-6)
def test_multiplication(self):
qr_1 = Quaternion(1., 2., 3., 4.)
qt_1 = Quaternion(1., 3., 3., 0.)
dq_1 = DualQuaternion(qr_1, qt_1)
qr_2 = Quaternion(1., 4., 5., 1.)
qt_2 = Quaternion(-4., 2., 3., 0.)
dq_2 = DualQuaternion(qr_2, qt_2)
# TODO(ff): Check this by hand. (And check if we shouldn't always get 0
# scalars for the rotational quaternion of the dual quaternion).
dq_expected = DualQuaternion(
dq_1.q_rot * dq_2.q_rot,
dq_1.q_rot * dq_2.q_dual + dq_1.q_dual * dq_2.q_rot)
npt.assert_allclose((dq_1 * dq_2).dq, dq_expected.dq)
def test_conjugate_identity(self):
qr = Quaternion(1, 2, 3, 4)
qt = Quaternion(1, 3, 3, 0)
dq = DualQuaternion(qr, qt)
# TODO(ff): Here we should use dq*dq.conjugate() in one
# place.
q_rot_identity = dq.q_rot * dq.q_rot.conjugate()
q_dual_identity = (dq.q_rot.conjugate() * dq.q_dual +
dq.q_dual.conjugate() * dq.q_rot)
identity_dq_expected = DualQuaternion(q_rot_identity, q_dual_identity)
npt.assert_allclose((dq * dq.conjugate()).dq,
identity_dq_expected.dq,
atol=1e-6)
def compute_orientation_mean_and_rmse_relative(q_AB_list, q_AC_list):
assert q_AB_list.shape == q_AC_list.shape
assert q_AC_list.shape[1] == 4
orientation_errors_rad = np.zeros(q_AB_list.shape[0] - 1)
for idx in range(q_AB_list.shape[0] - 1):
q_ABk = Quaternion(q_AB_list[idx, 0], q_AB_list[idx, 1],
q_AB_list[idx, 2], q_AB_list[idx, 3])
q_ACk = Quaternion(q_AC_list[idx, 0], q_AC_list[idx, 1],
q_AC_list[idx, 2], q_AC_list[idx, 3])
q_ABkp1 = Quaternion(q_AB_list[idx + 1, 0], q_AB_list[idx + 1, 1],
q_AB_list[idx + 1, 2], q_AB_list[idx + 1, 3])
q_ACkp1 = Quaternion(q_AC_list[idx + 1, 0], q_AC_list[idx + 1, 1],
q_AC_list[idx + 1, 2], q_AC_list[idx + 1, 3])
q_Bk_Bkp1 = q_ABk.inverse() * q_ABkp1
q_Ck_Ckp1 = q_ACk.inverse() * q_ACkp1
q_error = q_Bk_Bkp1.inverse() * q_Ck_Ckp1
angle_error = q_error.angle_axis()[3]
if angle_error > np.pi:
angle_error -= 2 * np.pi
assert angle_error <= np.pi
assert angle_error >= -np.pi
orientation_errors_rad[idx] = np.abs(angle_error)
return ErrorStatistics(orientation_errors_rad)
def test_normalize(self):
qr = Quaternion(1, 2, 3, 4)
qt = Quaternion(1, 3, 3, 0)
dq = DualQuaternion(qr, qt)
dq.normalize()
dq_normalized = np.array([
0.18257419, 0.36514837, 0.54772256, 0.73029674, 0.18257419,
0.54772256, 0.54772256, 0.
]).T
npt.assert_allclose(dq.dq, dq_normalized)
dq_2 = DualQuaternion.from_pose(1, 2, 3, 1, 1, 1, 1)
dq_2.normalize()
dq_2_normalized = np.array([0.5, 0.5, 0.5, 0.5, 0., 1., 0.5, -1.5]).T
npt.assert_allclose(dq_2.dq, dq_2_normalized)
def compute_loop_error(results_dq_H_E, num_poses_sets, visualize=False):
calibration_transformation_chain = []
# Add point at origin to represent the first coordinate
# frame in the chain of transformations.
calibration_transformation_chain.append(
DualQuaternion(Quaternion(0, 0, 0, 1), Quaternion(0, 0, 0, 0)))
# Add first transformation
calibration_transformation_chain.append(results_dq_H_E[0])
# Create chain of transformations from the first frame to the last.
i = 0
idx = 0
while i < (num_poses_sets - 2):
idx += (num_poses_sets - i - 1)
calibration_transformation_chain.append(results_dq_H_E[idx])
i += 1
# Add inverse of first to last frame to close the loop.
calibration_transformation_chain.append(results_dq_H_E[num_poses_sets -
2].inverse())
# Check loop.
assert len(calibration_transformation_chain) == (num_poses_sets + 1), (
len(calibration_transformation_chain), (num_poses_sets + 1))
# Chain the transformations together to get points we can plot.
poses_to_plot = []
dq_tmp = DualQuaternion(Quaternion(0, 0, 0, 1), Quaternion(0, 0, 0, 0))
for i in range(0, len(calibration_transformation_chain)):
dq_tmp *= calibration_transformation_chain[i]
poses_to_plot.append(dq_tmp.to_pose())
(loop_error_position,
loop_error_orientation) = compute_pose_error(poses_to_plot[0],
poses_to_plot[-1])
print("Error when closing the loop of hand eye calibrations - position: {}"
" m orientation: {} deg".format(loop_error_position,
loop_error_orientation))
if visualize:
assert len(poses_to_plot) == len(calibration_transformation_chain)
plot_poses([np.array(poses_to_plot)],
plot_arrows=True,
title="Hand-Eye Calibration Results - Closing The Loop")
# Compute error of loop.
return (loop_error_position, loop_error_orientation)
def csv_row_data_to_transformation(A_p_AB, q_AB_np):
"""
Constructs a proper transformation matrix out of the given translation vector
and quaternion.
Returns: 4x4 transformation matrix.
"""
assert (math.fabs(1.0 - np.linalg.norm(q_AB_np)) < 1e-8)
T_AB = np.identity(4)
q_AB = Quaternion(q=q_AB_np)
R_AB = q_AB.to_rotation_matrix()
T_AB[0:3, 0:3] = R_AB
T_AB[0:3, 3] = A_p_AB
return T_AB
def compute_orientation_mean_and_rmse(q_AB_list, q_AC_list):
assert q_AB_list.shape == q_AC_list.shape
assert (q_AC_list.shape[1] == 4)
orientation_errors_rad = np.zeros(q_AB_list.shape[0])
for idx in range(q_AB_list.shape[0]):
q_AB_xyzw = q_AB_list[idx, :]
q_AC_xyzw = q_AC_list[idx, :]
q_AB = Quaternion(q_AB_xyzw[0], q_AB_xyzw[1], q_AB_xyzw[2],
q_AB_xyzw[3])
q_AC = Quaternion(q_AC_xyzw[0], q_AC_xyzw[1], q_AC_xyzw[2],
q_AC_xyzw[3])
q_BC = q_AB.inverse() * q_AC
orientation_errors_rad[idx] = q_BC.angle_axis()[3]
return np.mean(orientation_errors_rad), RMSE(orientation_errors_rad)
def interpolate_poses_from_samples(time_stamped_poses, samples):
"""
Interpolate time stamped poses at the time stamps provided in samples.
The poses are expected in the following format:
[timestamp [s], x [m], y [m], z [m], qx, qy, qz, qw]
We apply linear interpolation to the position and use SLERP for
the quaternion interpolation.
"""
# Extract the quaternions from the poses.
quaternions = []
for pose in time_stamped_poses[:, 1:]:
quaternions.append(Quaternion(q=pose[3:]))
# interpolate the quaternions.
quaternions_interp = resample_quaternions_from_samples(
time_stamped_poses[:, 0], quaternions, samples)
# Interpolate the position and assemble the full aligned pose vector.
num_poses = samples.shape[0]
aligned_poses = np.zeros((num_poses, time_stamped_poses.shape[1]))
aligned_poses[:, 0] = samples[:]
for i in [1, 2, 3]:
aligned_poses[:, i] = np.interp(
samples,
np.asarray(time_stamped_poses[:, 0]).ravel(),
np.asarray(time_stamped_poses[:, i]).ravel())
for i in range(0, num_poses):
aligned_poses[i, 4:8] = quaternions_interp[i].q
return aligned_poses.copy()
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 test_slerp(self):
q_1 = Quaternion(1, 2, 3, 4)
q_2 = Quaternion(2, 3, 4, 5)
q_1.normalize()
q_2.normalize()
q_expected = np.array([
0.22772264489951, 0.38729833462074169, 0.54687402434197,
0.70644971406320634
]).T
npt.assert_allclose(quaternion_slerp(q_1, q_2, 0.5).q, q_expected)
def test_nlerp(self):
q_1 = Quaternion(1, 2, 3, 4)
q_2 = Quaternion(2, 3, 4, 5)
q_1.normalize()
q_2.normalize()
q_expected = np.array([0.22772264, 0.38729833, 0.54687402,
0.70644971]).T
npt.assert_allclose(quaternion_nlerp(q_1, q_2, 0.5).q,
q_expected,
atol=1e-6)
def test_lerp(self):
q_1 = Quaternion(1, 2, 3, 4)
q_2 = Quaternion(2, 3, 4, 5)
q_1.normalize()
q_2.normalize()
q_expected = np.array([0.22736986, 0.38669833, 0.54602681,
0.70535528]).T
npt.assert_allclose(quaternion_lerp(q_1, q_2, 0.5).q,
q_expected,
atol=1e-6)
def spoil_initial_guess(time_offset_initial_guess, dq_H_E_initial_guess, angular_offset_range,
translation_offset_range, time_offset_range):
""" Apply a random perturbation to the calibration."""
random_time_offset_offset = np.random.uniform(
time_offset_range[0], time_offset_range[1]) * random.choice([-1., 1.])
# Get a random unit vector
random_translation_offset = np.random.uniform(-1.0, 1.0, 3)
random_translation_offset /= np.linalg.norm(random_translation_offset)
assert np.isclose(np.linalg.norm(random_translation_offset), 1., atol=1e-8)
# Scale unit vector to a random length between 0 and max_translation_offset.
random_translation_length = np.random.uniform(
translation_offset_range[0], translation_offset_range[1])
random_translation_offset *= random_translation_length
# Get orientation offset of at most max_angular_offset.
random_quaternion_offset = Quaternion.get_random(
angular_offset_range[0], angular_offset_range[1])
translation_initial_guess = dq_H_E_initial_guess.to_pose()[0: 3]
orientation_initial_guess = dq_H_E_initial_guess.q_rot
time_offset_spoiled = time_offset_initial_guess + random_time_offset_offset
translation_spoiled = random_translation_offset + translation_initial_guess
orientation_spoiled = random_quaternion_offset * orientation_initial_guess
# Check if we spoiled correctly.
random_angle_offset = angle_between_quaternions(
orientation_initial_guess, orientation_spoiled)
assert random_angle_offset <= angular_offset_range[1]
assert random_angle_offset >= angular_offset_range[0]
assert np.linalg.norm(translation_spoiled -
translation_initial_guess) <= translation_offset_range[1]
assert np.linalg.norm(translation_spoiled -
translation_initial_guess) >= translation_offset_range[0]
# Get random orientation that distorts the original orientation by at most
# max_angular_offset rad.
dq_H_E_spoiled = DualQuaternion.from_pose(
translation_spoiled[0], translation_spoiled[1], translation_spoiled[2],
orientation_spoiled.q[0], orientation_spoiled.q[1],
orientation_spoiled.q[2], orientation_spoiled.q[3])
print("dq_H_E_initial_guess: {}".format(dq_H_E_initial_guess.dq))
print("dq_H_E_spoiled: {}".format(dq_H_E_spoiled.dq))
print("time_offset_initial_guess: {}".format(time_offset_initial_guess))
print("time_offset_spoiled: {}".format(time_offset_spoiled))
print("random_quaternion_offset: {}".format(random_quaternion_offset.q))
return (time_offset_spoiled, dq_H_E_spoiled,
(random_translation_offset, random_angle_offset, random_time_offset_offset))
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 generate_test_path(n_samples,
include_outliers=False,
outlier_probability=0.1,
include_noise=False,
noise_sigma_trans=0.01,
noise_sigma_rot=0.1):
# Create a sine for x, cos for y and linear motion for z.
# Rotate around the curve, while keeping the x-axis perpendicular to the
# curve.
dual_quaternions = []
t = np.linspace(0, 1, num=n_samples)
x = 10.0 + np.cos(4 * np.pi * t)
y = -5.0 + -np.sin(4 * np.pi * t) * 4
z = 0.5 + t * 5
theta = 4 * np.pi * t
outliers = []
for i in range(n_samples):
q_tmp = tf.transformations.quaternion_from_euler(
0., -theta[i] * 0.1, -theta[i], 'rxyz')
if include_outliers:
if np.random.rand() < outlier_probability:
outliers.append(i)
q_tmp = np.random.rand(4)
x[i] = np.random.rand() * max(x) * (
-1 if np.random.rand() < 0.5 else 0)
y[i] = np.random.rand() * max(y) * (
-1 if np.random.rand() < 0.5 else 0)
z[i] = np.random.rand() * max(z) * (
-1 if np.random.rand() < 0.5 else 0)
if include_noise:
# Add zero mean gaussian noise with sigma noise_sigma.
x[i] += np.random.normal(0.0, noise_sigma_trans)
y[i] += np.random.normal(0.0, noise_sigma_trans)
z[i] += np.random.normal(0.0, noise_sigma_trans)
# TODO(ff): Fix this, there should only be 3 random numbers drawn.
axis_noise_x = np.random.normal(0.0, noise_sigma_rot)
axis_noise_y = np.random.normal(0.0, noise_sigma_rot)
axis_noise_z = np.random.normal(0.0, noise_sigma_rot)
angle_noise = np.pi * np.random.normal(0.0, noise_sigma_rot)
q_noise = Quaternion.from_angle_axis(
angle_noise, (axis_noise_x, axis_noise_y, axis_noise_z))
q_noise.normalize()
q_noise_free = Quaternion(q=q_tmp)
q = q_noise * q_noise_free * q_noise.inverse()
else:
q = Quaternion(q=q_tmp)
q.normalize()
if q.w < 0.0:
q = -q
dq = DualQuaternion.from_pose(x[i], y[i], z[i], q.x, q.y, q.z, q.w)
dual_quaternions.append(dq)
if include_outliers:
print("Included {} outliers at {} in the test data.".format(
len(outliers), outliers))
return dual_quaternions
def compute_pose_error(pose_A, pose_B):
"""
Compute the error norm of position and orientation.
"""
error_position = np.linalg.norm(pose_A[0:3] - pose_B[0:3], ord=2)
# Construct quaternions to compare.
quaternion_A = Quaternion(q=pose_A[3:7])
quaternion_A.normalize()
if quaternion_A.w < 0:
quaternion_A.q = -quaternion_A.q
quaternion_B = Quaternion(q=pose_B[3:7])
quaternion_B.normalize()
if quaternion_B.w < 0:
quaternion_B.q = -quaternion_B.q
# Sum up the square of the orientation angle error.
error_angle_rad = angle_between_quaternions(quaternion_A, quaternion_B)
error_angle_degrees = math.degrees(error_angle_rad)
if error_angle_degrees > 180.0:
error_angle_degrees = math.fabs(360.0 - error_angle_degrees)
return (error_position, error_angle_degrees)
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 test_conjugate(self):
qr = Quaternion(1., 2., 3., 4.)
qt = Quaternion(1., 3., 3., 5.)
dq = DualQuaternion(qr, qt)
dq_expected = np.array([-1., -2., -3., 4., -1., -3., -3., 5.]).T
npt.assert_allclose((dq.conjugate()).dq, dq_expected, atol=1e-6)
def test_division_with_scalar(self):
qr = Quaternion(0.5, 0.5, -0.5, 0.5)
qt = Quaternion(1., 3., 3., 0.)
dq = DualQuaternion(qr, qt)
dq_expected = np.array([0.25, 0.25, -0.25, 0.25, 0.5, 1.5, 1.5, 0.]).T
npt.assert_allclose((dq / 2.).dq, dq_expected)
def test_equality(self):
qr = Quaternion(1, 2, 3, 4)
qt = Quaternion(1, 3, 3, 1)
dq_1 = DualQuaternion(qr, qt)
dq_2 = DualQuaternion(qr, qt)
self.assertEqual(dq_1, dq_2)
def test_inverse(self):
qr = Quaternion(1, 2, 3, 4)
qt = Quaternion(5, 6, 7, 8)
dq = DualQuaternion(qr, qt)
identity_dq = np.array([0., 0., 0., 1., 0., 0., 0., 0.]).T
npt.assert_allclose((dq * dq.inverse()).dq, identity_dq, atol=1e-6)
