def object_to_world(voxels,
euler_angles_x,
euler_angles_y,
translation_vector,
target_volume_size=(128, 128, 128)):
"""Apply the transformations to the voxels and place them in world coords."""
scale_factor = 1.82 # object to world voxel space scale factor
translation_vector = tf.expand_dims(translation_vector, axis=-1)
sampling_points = tf.cast(sampling_points_from_3d_grid(target_volume_size),
tf.float32) # 128^3 X 3
transf_matrix_x = rotation_matrix_3d.from_euler(
euler_angles_x) # [B, 3, 3]
transf_matrix_y = rotation_matrix_3d.from_euler(
euler_angles_y) # [B, 3, 3]
transf_matrix = tf.matmul(transf_matrix_x, transf_matrix_y) # [B, 3, 3]
transf_matrix = transf_matrix * scale_factor # [B, 3, 3]
sampling_points = tf.matmul(transf_matrix,
tf.transpose(sampling_points)) # [B, 3, N]
translation_vector = tf.matmul(transf_matrix,
translation_vector) # [B, 3, 1]
sampling_points = sampling_points - translation_vector
sampling_points = tf.linalg.matrix_transpose(sampling_points)
sampling_points = sampling_points_to_voxel_index(sampling_points,
target_volume_size)
sampling_points = tf.cast(sampling_points, tf.float32)
interpolated_points = trilinear.interpolate(voxels, sampling_points)
interpolated_voxels = tf.reshape(interpolated_points,
[-1] + list(target_volume_size) + [4])
return interpolated_voxels
def test_from_quaternion_random(self):
"""Checks that Euler angles can be retrieved from quaternions."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
random_matrix = rotation_matrix_3d.from_euler(random_euler_angles)
random_quaternion = quaternion.from_rotation_matrix(random_matrix)
predicted_angles = euler.from_quaternion(random_quaternion)
predicted_matrix = rotation_matrix_3d.from_euler(predicted_angles)
self.assertAllClose(random_matrix, predicted_matrix, atol=2e-3)
def test_from_axis_angle_gimbal(self, gimbal_configuration):
"""Checks that from_axis_angle works when Ry = pi/2 or -pi/2."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
random_euler_angles[..., 1] = gimbal_configuration
random_matrix = rotation_matrix_3d.from_euler(random_euler_angles)
random_axis, random_angle = axis_angle.from_euler(random_euler_angles)
predicted_random_angles = euler.from_axis_angle(random_axis, random_angle)
reconstructed_random_matrices = rotation_matrix_3d.from_euler(
predicted_random_angles)
self.assertAllClose(reconstructed_random_matrices, random_matrix, atol=1e-3)
def object_rotation_in_blender_world(voxels, object_rotation):
"""Rotate the voxels as in blender world."""
euler_angles = np.array([0, 0, 1], dtype=np.float32)*np.deg2rad(90)
object_correction_matrix = rotation_matrix_3d.from_euler(euler_angles)
euler_angles = np.array([0, 1, 0], dtype=np.float32)*(-object_rotation)
object_rotation_matrix = rotation_matrix_3d.from_euler(euler_angles)
euler_angles_blender = np.array([1, 0, 0], dtype=np.float32)*np.deg2rad(-90)
blender_object_correction_matrix = \
rotation_matrix_3d.from_euler(euler_angles_blender)
transformation_matrix = tf.matmul(tf.matmul(object_correction_matrix,
object_rotation_matrix),
blender_object_correction_matrix)
return transform_volume(voxels, transformation_matrix)
def test_interpolation_preset(self):
"""Tests whether interpolation results are correct."""
batch_dim_size = np.random.randint(0, 4)
batch_dims = list(np.random.randint(1, 10, size=batch_dim_size))
cube_single_dim = np.random.randint(3, 10)
cube_dims = [cube_single_dim, cube_single_dim, cube_single_dim]
num_channels = [np.random.randint(1, 10)]
combined_dims = batch_dims + cube_dims + num_channels
voxels_in = _generate_voxel_cube(combined_dims, "horizontal")
euler_angles = np.zeros(batch_dims + [3])
euler_angles[..., 2] = np.pi / 2.
voxels_out = _generate_voxel_cube(combined_dims, "vertical")
transformation_matrix = rotation_matrix_3d.from_euler(euler_angles)
grid_size = (cube_single_dim, cube_single_dim, cube_single_dim)
sampling_points = _sampling_points_from_grid(grid_size)
sampling_points = tf.matmul(transformation_matrix,
tf.transpose(a=sampling_points))
sampling_points = _transpose_last_two_dims(sampling_points)
sampling_points = _sampling_points_in_volume(sampling_points,
voxels_in.shape[-4:-1])
voxels_out = tf.reshape(
voxels_out, batch_dims + [cube_single_dim**3] + num_channels)
self.assert_output_is_correct(trilinear.interpolate,
(voxels_in, sampling_points),
(voxels_out, ),
tile=False)
def test_is_valid_random(self):
"""Tests that is_valid works as intended."""
random_euler_angle = test_helpers.generate_random_test_euler_angles()
tensor_tile = random_euler_angle.shape[:-1]
rotation_matrix = rotation_matrix_3d.from_euler(random_euler_angle)
pred_normalized = rotation_matrix_3d.is_valid(rotation_matrix)
with self.subTest(name="all_normalized"):
self.assertAllEqual(pred_normalized,
np.ones(shape=tensor_tile + (1, ), dtype=bool))
with self.subTest(name="non_orthonormal"):
test_matrix = np.array([[2., 0., 0.], [0., 0.5, 0], [0., 0., 1.]])
pred_normalized = rotation_matrix_3d.is_valid(test_matrix)
self.assertAllEqual(pred_normalized,
np.zeros(shape=(1, ), dtype=bool))
with self.subTest(name="negative_orthonormal"):
test_matrix = np.array([[1., 0., 0.], [0., -1., 0.], [0., 0., 1.]])
pred_normalized = rotation_matrix_3d.is_valid(test_matrix)
self.assertAllEqual(pred_normalized,
np.zeros(shape=(1, ), dtype=bool))
def change_coordinate_system(points3d,
rotations=(0., 0., 0.),
scale=(1., 1., 1.),
name="change_coordinate_system"):
"""Change coordinate system.
Args:
points3d: A tensor of shape `[A1, ..., An, M, 3]` containing the
3D position of M points.
rotations: A tuple containing the X, Y, Z axis rotation.
scale: A tuple containing the X, Y, Z axis scale.
name: A name for this op. Defaults to "change_coordinate_system".
Returns:
[type]: [description]
"""
with tf.name_scope(name):
points3d = tf.convert_to_tensor(points3d)
rotation = tf.convert_to_tensor(rotations)
scale = tf.convert_to_tensor(scale)
rotation_matrix = rotation_matrix_3d.from_euler(rotation)
scaling_matrix = scale * tf.eye(3, 3)
target_shape = [1] * (len(points3d.get_shape().as_list()) - 2) + [3, 3]
transformation = tf.matmul(scaling_matrix, rotation_matrix)
transformation = tf.reshape(transformation, target_shape)
return tf.linalg.matrix_transpose(
tf.matmul(transformation, tf.linalg.matrix_transpose(points3d)))
def test_from_rotation_matrix_gimbal(self):
"""Testing that Euler angles can be retrieved in Gimbal lock."""
angles = test_helpers.generate_random_test_euler_angles()
angles[..., 1] = np.pi / 2.
matrix = rotation_matrix_3d.from_euler(angles)
predicted_angles = euler.from_rotation_matrix(matrix)
reconstructed_matrices = rotation_matrix_3d.from_euler(predicted_angles)
self.assertAllClose(reconstructed_matrices, matrix, rtol=1e-3)
angles[..., 1] = -np.pi / 2.
matrix = rotation_matrix_3d.from_euler(angles)
predicted_angles = euler.from_rotation_matrix(matrix)
reconstructed_matrices = rotation_matrix_3d.from_euler(predicted_angles)
self.assertAllClose(reconstructed_matrices, matrix, rtol=1e-3)
def test_from_euler_normalized_random(self):
"""Tests that euler angles can be converted to rotation matrices."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
matrix = rotation_matrix_3d.from_euler(random_euler_angles)
self.assertAllEqual(rotation_matrix_3d.is_valid(matrix),
np.ones(random_euler_angles.shape[0:-1] + (1, )))
def test_from_rotation_matrix_preset(self):
"""Tests that Euler angles can be retrieved from rotation matrices."""
matrix = test_helpers.generate_preset_test_rotation_matrices_3d()
predicted_angles = euler.from_rotation_matrix(matrix)
reconstructed_matrices = rotation_matrix_3d.from_euler(predicted_angles)
self.assertAllClose(reconstructed_matrices, matrix, rtol=1e-3)
def test_from_euler_jacobian_random(self):
"""Test the Jacobian of the from_euler function."""
x_init = test_helpers.generate_random_test_euler_angles()
x = tf.convert_to_tensor(value=x_init)
y = rotation_matrix_3d.from_euler(x)
self.assert_jacobian_is_correct(x, x_init, y)
def generate_preset_test_rotation_matrices_3d():
"""Generates pre-set test 3d rotation matrices."""
angles = generate_preset_test_euler_angles()
preset_rotation_matrix = rotation_matrix_3d.from_euler(angles)
if tf.executing_eagerly():
return np.array(preset_rotation_matrix)
with tf.compat.v1.Session() as sess:
return np.array(sess.run([preset_rotation_matrix]))
def test_assert_rotation_matrix_normalized_passthrough(self):
"""Checks that the assert is a passthrough when the flag is False."""
angles = test_helpers.generate_preset_test_euler_angles()
matrix_input = rotation_matrix_3d.from_euler(angles)
matrix_output = rotation_matrix_3d.assert_rotation_matrix_normalized(
matrix_input)
self.assertTrue(matrix_input is matrix_output) # pylint: disable=g-generic-assert
def test_from_quaternion_preset(self):
"""Checks that Euler angles can be retrieved from quaternions."""
preset_euler_angles = test_helpers.generate_preset_test_euler_angles()
preset_matrix = rotation_matrix_3d.from_euler(preset_euler_angles)
preset_quaternion = quaternion.from_euler(preset_euler_angles)
predicted_matrix = rotation_matrix_3d.from_quaternion(preset_quaternion)
self.assertAllClose(preset_matrix, predicted_matrix, atol=2e-3)
def fix_rotation_matrix(rotation_matrix, object_class):
fix_angle = tf_utils.euler_from_rotation_matrix(rotation_matrix, 1)
if object_class == 2: # table
fix_angle = tf.math.floormod(fix_angle, math.pi)
elif object_class == 3 or object_class == 4: # bottle, bowl
fix_angle = tf.constant(0.0)
euler_angles_fixed = tf.stack([0.0, fix_angle, 0.0])
rotation_matrix = rotation_matrix_3d.from_euler(euler_angles_fixed)
return rotation_matrix
def test_from_rotation_matrix_normalized_random(self):
"""Tests that from_rotation_matrix returns normalized axis-angles."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
matrix = rotation_matrix_3d.from_euler(random_euler_angles)
axis, angle = axis_angle.from_rotation_matrix(matrix)
self.assertAllEqual(
axis_angle.is_normalized(axis, angle), np.ones(angle.shape, dtype=bool))
def test_from_axis_angle_preset(self):
"""Checks that Euler angles can be retrieved from axis-angle."""
preset_euler_angles = test_helpers.generate_preset_test_euler_angles()
random_matrix = rotation_matrix_3d.from_euler(preset_euler_angles)
random_axis, random_angle = axis_angle.from_euler(preset_euler_angles)
predicted_matrix = rotation_matrix_3d.from_axis_angle(
random_axis, random_angle)
self.assertAllClose(random_matrix, predicted_matrix, atol=1e-3)
def test_from_quaternion_random(self):
"""Tests conversion to matrix."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
random_quaternions = quaternion.from_euler(random_euler_angles)
random_rotation_matrices = rotation_matrix_3d.from_euler(
random_euler_angles)
self.assertAllClose(random_rotation_matrices,
rotation_matrix_3d.from_quaternion(random_quaternions))
def test_from_rotation_matrix_random(self):
"""Tests that Euler angles can be retrieved from rotation matrices."""
matrix = test_helpers.generate_random_test_rotation_matrix_3d()
predicted_angles = euler.from_rotation_matrix(matrix)
# There is not a unique mapping from rotation matrices to Euler angles. The
# following constructs the rotation matrices from the `predicted_angles` and
# compares them with `matrix`.
reconstructed_matrices = rotation_matrix_3d.from_euler(predicted_angles)
self.assertAllClose(reconstructed_matrices, matrix, rtol=1e-3)
def test_from_quaternion_gimbal(self, gimbal_configuration):
"""Checks that from_quaternion works when Ry = pi/2 or -pi/2."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
random_euler_angles[..., 1] = gimbal_configuration
random_quaternion = quaternion.from_euler(random_euler_angles)
random_matrix = rotation_matrix_3d.from_euler(random_euler_angles)
reconstructed_random_matrices = rotation_matrix_3d.from_quaternion(
random_quaternion)
self.assertAllClose(reconstructed_random_matrices, random_matrix, atol=2e-3)
def test_inverse_normalized_random(self):
"""Checks that inverted rotation matrices are valid rotations."""
random_euler_angle = test_helpers.generate_random_test_euler_angles()
tensor_tile = random_euler_angle.shape[:-1]
random_matrix = rotation_matrix_3d.from_euler(random_euler_angle)
predicted_invert_random_matrix = rotation_matrix_3d.inverse(random_matrix)
self.assertAllEqual(
rotation_matrix_3d.is_valid(predicted_invert_random_matrix),
np.ones(tensor_tile + (1,)))
def test_rotate_vs_rotate_quaternion_random(self):
"""Tests that the rotate provide the same results as quaternion.rotate."""
random_euler_angle = test_helpers.generate_random_test_euler_angles()
tensor_tile = random_euler_angle.shape[:-1]
random_matrix = rotation_matrix_3d.from_euler(random_euler_angle)
random_quaternion = quaternion.from_rotation_matrix(random_matrix)
random_point = np.random.normal(size=tensor_tile + (3, ))
ground_truth = quaternion.rotate(random_point, random_quaternion)
prediction = rotation_matrix_3d.rotate(random_point, random_matrix)
self.assertAllClose(ground_truth, prediction, rtol=1e-6)
def test_from_euler_random(self):
"""Tests that quaternions can be constructed from Euler angles."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
tensor_shape = random_euler_angles.shape[:-1]
random_matrix = rotation_matrix_3d.from_euler(random_euler_angles)
random_quaternion = quaternion.from_euler(random_euler_angles)
random_point = np.random.normal(size=tensor_shape + (3,))
rotated_with_matrix = rotation_matrix_3d.rotate(random_point, random_matrix)
rotated_with_quaternion = quaternion.rotate(random_point, random_quaternion)
self.assertAllClose(rotated_with_matrix, rotated_with_quaternion)
def test_from_rotation_matrix_random(self):
"""Tests that from_rotation_matrix produces the expected quaternions."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
random_rotation_matrix_3d = rotation_matrix_3d.from_euler(
random_euler_angles)
groundtruth = rotation_matrix_3d.from_quaternion(
quaternion.from_euler(random_euler_angles))
prediction = rotation_matrix_3d.from_quaternion(
quaternion.from_rotation_matrix(random_rotation_matrix_3d))
self.assertAllClose(groundtruth, prediction)
def test_assert_rotation_matrix_normalized_preset(self, dtype):
"""Checks that assert_normalized function works as expected."""
angles = test_helpers.generate_preset_test_euler_angles().astype(dtype)
matrix = rotation_matrix_3d.from_euler(angles)
matrix_rescaled = matrix * 1.01
matrix_normalized = rotation_matrix_3d.assert_rotation_matrix_normalized(
matrix)
self.evaluate(matrix_normalized)
with self.assertRaises(tf.errors.InvalidArgumentError): # pylint: disable=g-error-prone-assert-raises
self.evaluate(rotation_matrix_3d.assert_rotation_matrix_normalized(
matrix_rescaled))
def test_from_euler_random(self):
"""Tests that from_euler allows to perform the expect rotation of points."""
random_euler_angles = test_helpers.generate_random_test_euler_angles()
tensor_shape = random_euler_angles.shape[:-1]
random_point = np.random.normal(size=tensor_shape + (3,))
random_matrix = rotation_matrix_3d.from_euler(random_euler_angles)
random_axis, random_angle = axis_angle.from_euler(random_euler_angles)
rotated_with_matrix = rotation_matrix_3d.rotate(random_point, random_matrix)
rotated_with_axis_angle = axis_angle.rotate(random_point, random_axis,
random_angle)
self.assertAllClose(rotated_with_matrix, rotated_with_axis_angle)
def test_from_euler_with_small_angles_approximation_random(self):
"""Tests small_angles approximation by comparing to exact calculation."""
# Only generate small angles. For a test tolerance of 1e-3, 0.16 was found
# empirically to be the range where the small angle approximation works.
random_euler_angles = test_helpers.generate_random_test_euler_angles(
min_angle=-0.16, max_angle=0.16)
exact_matrix = rotation_matrix_3d.from_euler(random_euler_angles)
approximate_matrix = (
rotation_matrix_3d.from_euler_with_small_angles_approximation(
random_euler_angles))
self.assertAllClose(exact_matrix, approximate_matrix, atol=1e-3)
def test_inverse_random(self):
"""Checks that inverting rotated points results in no transformation."""
random_euler_angle = test_helpers.generate_random_test_euler_angles()
tensor_tile = random_euler_angle.shape[:-1]
random_matrix = rotation_matrix_3d.from_euler(random_euler_angle)
random_point = np.random.normal(size=tensor_tile + (3,))
rotated_random_points = rotation_matrix_3d.rotate(random_point,
random_matrix)
predicted_invert_random_matrix = rotation_matrix_3d.inverse(random_matrix)
predicted_invert_rotated_random_points = rotation_matrix_3d.rotate(
rotated_random_points, predicted_invert_random_matrix)
self.assertAllClose(
random_point, predicted_invert_rotated_random_points, rtol=1e-6)
def matrix_from_angles(rot):
"""Create a rotation matrix from a triplet of rotation angles.
Args:
rot: a tf.Tensor of shape [..., 3], where the last dimension is the rotation
angles, along x, y, and z.
Returns:
A tf.tensor of shape [..., 3, 3], where the last two dimensions are the
rotation matrix.
This function mimics _euler2mat from struct2depth/project.py, for backward
compatibility, but wraps tensorflow_graphics instead of reimplementing it.
The negation and transposition are needed to bridge the differences between
the two.
"""
rank = tf.rank(rot)
# Swap the two last dimensions
perm = tf.concat([tf.range(rank - 1), [rank], [rank - 1]], axis=0)
return tf.transpose(rotation_matrix_3d.from_euler(-rot), perm)
def test_from_euler_random(self):
"""Tests that Euler angles produce the same result as axis-angle."""
angles = test_helpers.generate_random_test_euler_angles()
matrix = rotation_matrix_3d.from_euler(angles)
tensor_tile = angles.shape[:-1]
x_axis = np.tile(td.AXIS_3D_X, tensor_tile + (1,))
y_axis = np.tile(td.AXIS_3D_Y, tensor_tile + (1,))
z_axis = np.tile(td.AXIS_3D_Z, tensor_tile + (1,))
x_angle = np.expand_dims(angles[..., 0], axis=-1)
y_angle = np.expand_dims(angles[..., 1], axis=-1)
z_angle = np.expand_dims(angles[..., 2], axis=-1)
x_rotation = rotation_matrix_3d.from_axis_angle(x_axis, x_angle)
y_rotation = rotation_matrix_3d.from_axis_angle(y_axis, y_angle)
z_rotation = rotation_matrix_3d.from_axis_angle(z_axis, z_angle)
expected_matrix = tf.matmul(z_rotation, tf.matmul(y_rotation, x_rotation))
self.assertAllClose(expected_matrix, matrix, rtol=1e-3)
