def from_axis_angle_translation(
axis: type_alias.TensorLike,
angle: type_alias.TensorLike,
translation_vector: type_alias.TensorLike,
name: str = "dual_quat_from_axis_angle_trans"
) -> type_alias.TensorLike:
"""Converts an axis-angle rotation and translation to a dual quaternion.
Note:
In the following, A1 to An are optional batch dimensions.
Args:
axis: A tensor of shape `[A1, ..., An, 3]`, where the last dimension
represents a normalized axis.
angle: A tensor of shape `[A1, ..., An, 1]`, where the last dimension
represents an angle.
translation_vector: A `[A1, ..., An, 3]`-tensor, where the last dimension
represents a translation vector.
name: A name for this op that defaults to "dual_quat_from_axis_angle_trans".
Returns:
A `[A1, ..., An, 8]`-tensor, where the last dimension represents a
normalized dual quaternion.
Raises:
ValueError: If the shape of `axis`, `angle`, or `translation_vector`
is not supported.
"""
with tf.name_scope(name):
axis = tf.convert_to_tensor(value=axis)
angle = tf.convert_to_tensor(value=angle)
translation_vector = tf.convert_to_tensor(value=translation_vector)
shape.check_static(tensor=axis,
tensor_name="axis",
has_dim_equals=(-1, 3))
shape.check_static(tensor=angle,
tensor_name="angle",
has_dim_equals=(-1, 1))
shape.check_static(tensor=translation_vector,
tensor_name="translation_vector",
has_dim_equals=(-1, 3))
shape.compare_batch_dimensions(tensors=(axis, angle,
translation_vector),
last_axes=-2,
broadcast_compatible=True)
scalar_shape = tf.concat((tf.shape(translation_vector)[:-1], (1, )),
axis=-1)
dtype = translation_vector.dtype
quaternion_rotation = quaternion.from_axis_angle(axis, angle)
quaternion_translation = tf.concat(
(translation_vector, tf.zeros(scalar_shape, dtype)), axis=-1)
dual_quaternion_dual_part = 0.5 * quaternion.multiply(
quaternion_translation, quaternion_rotation)
return tf.concat((quaternion_rotation, dual_quaternion_dual_part),
axis=-1)
def predict_the_pose(pos_init, quat_init, xy_pred, z_pred, quat_pred,
img_scaling, cam_mat):
img_center = tf.matmul(tf.expand_dims(pos_init, axis=1),
cam_mat,
transpose_b=True)
img_center = tf.reduce_sum(img_center, axis=1)
img_center = tf.div(img_center, tf.expand_dims(img_center[:, -1], axis=1))
xy_image = img_center[:, :2] + xy_pred / tf.expand_dims(img_scaling,
axis=-1)
z_model = pos_init[:, 2]
x_model_cam_frame = (xy_image[:, 0] - cam_mat[:, 0, 2]) / cam_mat[:, 0, 0]
x_model_cam_frame = tf.expand_dims(tf.multiply(x_model_cam_frame, z_model),
axis=-1)
y_model_cam_frame = (xy_image[:, 1] - cam_mat[:, 1, 2]) / cam_mat[:, 1, 1]
y_model_cam_frame = tf.expand_dims(tf.multiply(y_model_cam_frame, z_model),
axis=-1)
z_model = tf.expand_dims(z_model, axis=-1)
pose_model_lateral = tf.concat(
(x_model_cam_frame, y_model_cam_frame, z_model), axis=-1)
z_out = z_pred / 3000.0
pos_out = tf.multiply(tf.exp(z_out), pose_model_lateral)
quat = quat_pred / 10.0
quat_len = tf.expand_dims(tf.norm(quat, axis=-1), axis=-1)
quat_out = tf.div(quat, quat_len)
quat_out = quaternion.multiply(quat_out, quat_init)
return pos_out, quat_out
def to_rotation_translation(
dual_quaternion: type_alias.TensorLike,
name: str = "dual_quaternion_to_rot_trans"
) -> Tuple[tf.Tensor, tf.Tensor]:
"""Converts a dual quaternion into a quaternion for rotation and translation.
Args:
dual_quaternion: A `[A1, ..., An, 8]`-tensor, where the last dimension
represents a qual quaternion.
name: A name for this op that defaults to "dual_quaternion_to_rot_trans".
Returns:
A tuple with a `[A1, ..., An, 4]`-tensor for rotation in quaternion form,
and a `[A1, ..., An, 3]`-tensor for translation, in that order.
"""
with tf.name_scope(name):
dual_quaternion = tf.convert_to_tensor(value=dual_quaternion)
shape.check_static(tensor=dual_quaternion,
tensor_name="dual_quaternion",
has_dim_equals=(-1, 8))
rotation = dual_quaternion[..., 0:4]
translation = 2 * quaternion.multiply(dual_quaternion[..., 4:8],
quaternion.inverse(rotation))
translation = translation[..., 0:3]
return rotation, translation
def eval_accuracy(X, Y, Y_hat, X_train, Y_train, X_test, Y_test, t_treshold, r_threshold):
# Have to swap axes due to how tensorflow quaternions work
Yq = tf.transpose(Y[:4], [1, 0])
Yq_hat = tf.transpose(Y_hat[:4], [1, 0])
# Compute difference quaternion then swap axes back
diff = tfq.multiply(tfq.normalize(Yq), tfq.inverse(tfq.normalize(Yq_hat)))
diff = tf.transpose(diff, [1, 0])
# Compute angle difference in degrees
diffW = tf.clip_by_value(diff[3], clip_value_min=-1.0, clip_value_max=1.0)
angle_diff = tf_rad2deg(math.pi - tf.math.abs(2 * tf.math.acos(diffW) - math.pi))
correct_rot = tf.cast(tf.less(angle_diff, [r_threshold]), "float")
correct_trans = tf.cast(tf.less(tf.norm(Y[4:] - Y_hat[4:], axis = 0), [t_treshold]), "float")
t_accuracy = tf.reduce_mean(tf.cast(correct_trans, "float"))
r_accuracy = tf.reduce_mean(tf.cast(correct_rot, "float"))
accuracy = tf.reduce_mean(tf.cast(correct_trans * correct_rot, "float"))
print('\n--------------')
print("Evaluating accuracy with rotation threshold of " + str(r_threshold) + " and translation treshold of " + str(t_treshold))
print('')
print ("Train Accuracy:", accuracy.eval({X: X_train, Y: Y_train}))
print ("Test Accuracy:", accuracy.eval({X: X_test, Y: Y_test}))
print('')
print ("Train Accuracy on just translation:", t_accuracy.eval({X: X_train, Y: Y_train}))
print ("Test Accuracy on just translation:", t_accuracy.eval({X: X_test, Y: Y_test}))
print('')
print ("Train Accuracy on just rotation:", r_accuracy.eval({X: X_train, Y: Y_train}))
print ("Test Accuracy on just rotation:", r_accuracy.eval({X: X_test, Y: Y_test}))
print('\n--------------')
def multiply(dual_quaternion1: type_alias.TensorLike,
dual_quaternion2: type_alias.TensorLike,
name: str = "dual_quaternion_multiply") -> tf.Tensor:
"""Multiplies two dual quaternions.
Note:
In the following, A1 to An are optional batch dimensions.
Args:
dual_quaternion1: A TensorLike of shape `[A1, ..., An, 8]`, where the last
dimension represents a dual quaternion.
dual_quaternion2: A TensorLike of shape `[A1, ..., An, 8]`, where the last
dimension represents a dual quaternion.
name: A name for this op that defaults to "dual_quaternion_multiply".
Returns:
A tensor of shape `[A1, ..., An, 8]` representing dual quaternions.
"""
with tf.name_scope(name):
dual_quaternion1 = tf.convert_to_tensor(value=dual_quaternion1)
dual_quaternion2 = tf.convert_to_tensor(value=dual_quaternion2)
shape.check_static(tensor=dual_quaternion1,
tensor_name="dual_quaternion1",
has_dim_equals=(-1, 8))
shape.check_static(tensor=dual_quaternion2,
tensor_name="dual_quaternion2",
has_dim_equals=(-1, 8))
dual_quaternion1_real, dual_quaternion1_dual = tf.split(
dual_quaternion1, (4, 4), axis=-1)
dual_quaternion2_real, dual_quaternion2_dual = tf.split(
dual_quaternion2, (4, 4), axis=-1)
dual_quaternion_output_real = quaternion.multiply(
dual_quaternion1_real, dual_quaternion2_real)
dual_quaternion_output_dual = (
quaternion.multiply(dual_quaternion1_real, dual_quaternion2_dual) +
quaternion.multiply(dual_quaternion1_dual, dual_quaternion2_real))
return tf.concat(
(dual_quaternion_output_real, dual_quaternion_output_dual),
axis=-1)
def test_multiply_jacobian_preset(self):
"""Test the Jacobian of the multiply function."""
x_1_init = test_helpers.generate_preset_test_quaternions()
x_2_init = test_helpers.generate_preset_test_quaternions()
x_1 = tf.convert_to_tensor(value=x_1_init)
x_2 = tf.convert_to_tensor(value=x_2_init)
y = quaternion.multiply(x_1, x_2)
self.assert_jacobian_is_correct(x_1, x_1_init, y)
self.assert_jacobian_is_correct(x_2, x_2_init, y)
def test_inverse_random(self):
"""Tests that multiplying with the inverse gives identity."""
random_quaternion = test_helpers.generate_random_test_quaternions()
inverse_quaternion = quaternion.inverse(random_quaternion)
final_quaternion = quaternion.multiply(random_quaternion,
inverse_quaternion)
tensor_shape = random_quaternion.shape[:-1]
identity_quaternion = np.array((0.0, 0.0, 0.0, 1.0), dtype=np.float32)
identity_quaternion = np.tile(identity_quaternion, tensor_shape + (1,))
self.assertAllClose(final_quaternion, identity_quaternion, rtol=1e-3)
def from_rotation_translation(
rotation_quaternion: type_alias.TensorLike,
translation_vector: type_alias.TensorLike,
name: str = "dual_quaternion_from_rotation_translation") -> tf.Tensor:
"""Converts a rotation matrix and translation vector to a dual quaternion.
Warning:
This function is not smooth everywhere.
Note:
In the following, A1 to An are optional batch dimensions. Rotation is
applied first.
Args:
rotation_quaternion: A `[A1, ..., An, 4]`-tensor, where the last dimension
represents a rotation in the form a quaternion.
translation_vector: A `[A1, ..., An, 3]`-tensor, where the last dimension
represents a translation vector.
name: A name for this op that defaults to "dual_quaternion_from_rot_trans".
Returns:
A `[A1, ..., An, 8]`-tensor, where the last dimension represents a
normalized dual quaternion.
Raises:
ValueError: If the shape of `rotation_matrix` is not supported.
"""
with tf.name_scope(name):
rotation_quaternion = tf.convert_to_tensor(value=rotation_quaternion)
translation_vector = tf.convert_to_tensor(value=translation_vector)
shape.check_static(tensor=rotation_quaternion,
tensor_name="rotation_quaternion",
has_rank_greater_than=1,
has_dim_equals=(-1, 4))
shape.check_static(tensor=translation_vector,
tensor_name="translation_vector",
has_dim_equals=(-1, 3))
scalar_shape = tf.concat((tf.shape(translation_vector)[:-1], (1, )),
axis=-1)
dtype = translation_vector.dtype
quaternion_translation = tf.concat(
(translation_vector, tf.zeros(scalar_shape, dtype)), axis=-1)
dual_quaternion_dual_part = 0.5 * quaternion.multiply(
quaternion_translation, rotation_quaternion)
return tf.concat((rotation_quaternion, dual_quaternion_dual_part),
axis=-1)
def test_multiply_jacobian_random(self):
"""Test the Jacobian of the multiply function."""
tensor_dimensions = np.random.randint(low=1, high=3)
tensor_shape = np.random.randint(1, 10, size=(tensor_dimensions)).tolist()
x_1_init = test_helpers.generate_random_test_quaternions(tensor_shape)
x_2_init = test_helpers.generate_random_test_quaternions(tensor_shape)
x_1 = tf.convert_to_tensor(value=x_1_init)
x_2 = tf.convert_to_tensor(value=x_2_init)
y = quaternion.multiply(x_1, x_2)
self.assert_jacobian_is_correct(x_1, x_1_init, y)
self.assert_jacobian_is_correct(x_2, x_2_init, y)
def generate_preset_test_dual_quaternions():
"""Generates pre-set test quaternions."""
angles = generate_preset_test_euler_angles()
preset_quaternion_real = quaternion.from_euler(angles)
translations = generate_preset_test_translations()
translations = np.concatenate(
(translations / 2.0, np.zeros((np.ma.size(translations, 0), 1))),
axis=1)
preset_quaternion_translation = tf.convert_to_tensor(value=translations)
preset_quaternion_dual = quaternion.multiply(preset_quaternion_translation,
preset_quaternion_real)
preset_dual_quaternion = tf.concat(
(preset_quaternion_real, preset_quaternion_dual), axis=-1)
return preset_dual_quaternion
def generate_random_test_dual_quaternions():
"""Generates random test dual quaternions."""
angles = generate_random_test_euler_angles()
random_quaternion_real = quaternion.from_euler(angles)
min_translation = -3.0
max_translation = 3.0
translations = np.random.uniform(min_translation, max_translation,
angles.shape)
translations_quaternion_shape = np.asarray(translations.shape)
translations_quaternion_shape[-1] = 1
translations = np.concatenate(
(translations / 2.0, np.zeros(translations_quaternion_shape)), axis=-1)
random_quaternion_translation = tf.convert_to_tensor(value=translations)
random_quaternion_dual = quaternion.multiply(random_quaternion_translation,
random_quaternion_real)
random_dual_quaternion = tf.concat(
(random_quaternion_real, random_quaternion_dual), axis=-1)
return random_dual_quaternion
# actually want the center-to-edge distance, i.e. the 'square radius'.
source_size /= 2
# =============================================================================
# Make the source.
angle_type = "quaternion"
if angle_type == "vector":
source_angle = np.array((0.0, -source_y_offset, target_z_distance),
dtype=np.float64)
else:
rot1 = quaternion.from_euler((0.0, -PI / 2, 0.0))
rot2 = quaternion.from_euler(
tf.cast((math.atan2(source_y_offset, target_z_distance), 0.0, 0.0),
dtype=tf.float64))
source_angle = quaternion.multiply(rot2, rot1)
source_center = np.array(
(source_x_offset, source_y_offset, -target_z_distance), dtype=np.float64)
angular_distribution = distributions.SquareRankLambertianSphere(
ray_sample_count, source_angular_cutoff)
base_point_distribution = distributions.RandomUniformSquare(
source_size, sqrt_ray_sample_count)
source = sources.AngularSource(3,
source_center,
source_angle,
angular_distribution,
base_point_distribution,
wavelengths,
dense=False,
ray_length=100,
