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)