def test_interpolate_with_weights_vector_preset(self):
"""Compares interpolate to vector_weights + interpolate_with_weights."""
# Any quaternion is a valid vector
q1 = self._pick_random_quaternion()
q2 = q1 + tf.ones_like(q1)
weight1, weight2 = slerp.vector_weights(q1, q2, 0.75)
qf = slerp.interpolate_with_weights(q1, q2, weight1, weight2)
qi = slerp.interpolate(q1, q2, 0.75, method=slerp.InterpolationType.VECTOR)
self.assertAllClose(qf, qi, atol=1e-9)
def test_interpolate_with_weights_quaternion_preset(self):
"""Compares interpolate to quaternion_weights + interpolate_with_weights."""
q1 = self._pick_random_quaternion()
q2 = q1 + tf.ones_like(q1)
q1 = tf.nn.l2_normalize(q1, axis=-1)
q2 = tf.nn.l2_normalize(q2, axis=-1)
weight1, weight2 = slerp.quaternion_weights(q1, q2, 0.25)
qf = slerp.interpolate_with_weights(q1, q2, weight1, weight2)
qi = slerp.interpolate(
q1, q2, 0.25, method=slerp.InterpolationType.QUATERNION)
self.assertAllClose(qf, qi, atol=1e-9)
def interpolate_pose(ref_pose, src_pose):
# Interpolate rotation
ref_rot = ref_pose[:, :3, :3]
src_rot = src_pose[:, :3, :3]
ref_quat = tform.quaternion.from_rotation_matrix(ref_rot)
src_quat = tform.quaternion.from_rotation_matrix(src_rot)
out_quat = slerp.interpolate(ref_quat, src_quat, 0.5)
out_rot = tform.rotation_matrix_3d.from_quaternion(out_quat)
# Interpolate translation
ref_t = ref_pose[:, :3, 3:]
src_t = src_pose[:, :3, 3:]
out_t = 0.5 * ref_t + 0.5 * src_t
# Output
combined = tf.concat([out_rot, out_t], axis=2)
return tf.concat([combined, ref_pose[:, 3:, :]], axis=1)
def _vector_slerp_helper(self, q1, q2, p): """Calls interpolate function for vectors.""" return slerp.interpolate(q1, q2, p, slerp.InterpolationType.VECTOR)
def _quaternion_slerp_helper(self, q1, q2, p): """Calls interpolate function for quaternions.""" return slerp.interpolate(q1, q2, p, slerp.InterpolationType.QUATERNION)