def test_consecutive_transformations(self):
dq_1_2 = DualQuaternion.from_pose(0, 10, 1, 1, 0, 0, 0)
dq_2_3 = DualQuaternion.from_pose(2, 1, 3, 0, 1, 0, 0)
dq_1_3 = DualQuaternion.from_pose(2, 9, -2, 0, 0, 1, 0)
# Move coordinate frame dq_1 to dq_3
dq_1_3_computed = dq_1_2 * dq_2_3
npt.assert_allclose(dq_1_3_computed.dq, dq_1_3.dq)
def test_transforming_points(self):
dq_1_2 = DualQuaternion.from_pose(0, 10, 1, 1, 0, 0, 0)
dq_2_3 = DualQuaternion.from_pose(2, 1, 3, 0, 1, 0, 0)
dq_1_3 = DualQuaternion.from_pose(2, 9, -2, 0, 0, 1, 0)
dq_1_3_computed = dq_1_2 * dq_2_3
# Express point p (expressed in frame 1) in coordinate frame 3.
p_1 = np.array([1, 2, 3])
p_3_direct = dq_1_3.inverse().passive_transform_point(p_1)
p_3_consecutive = dq_1_3_computed.inverse().passive_transform_point(
p_1)
npt.assert_allclose(p_3_direct, p_3_consecutive)
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 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 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)
