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 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 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 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 from_pose(cls, x, y, z, rx, ry, rz, rw):
""" Create a normalized dual quaternion from a pose. """
qr = Quaternion(rx, ry, rz, rw)
qr.normalize()
qt = (Quaternion(x, y, z, 0) * qr) * 0.5
return cls(qr, qt)
def test_normalize(self):
q = Quaternion(1, 2, 3, 4)
q.normalize()
q_normalized = np.array(
[0.18257419, 0.36514837, 0.54772256, 0.73029674]).T
npt.assert_allclose(q.q, q_normalized)