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)