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)