def test_from_rotation_matrix_jacobian_random(self): """Test the Jacobian of the from_rotation_matrix function. Note: Preset angles are not tested as the gradient of tf.norm is NaN a 0. """ x_init = test_helpers.generate_random_test_rotation_matrix_3d() self.assert_jacobian_is_finite_fn( lambda x: axis_angle.from_rotation_matrix(x)[0], [x_init]) self.assert_jacobian_is_finite_fn( lambda x: axis_angle.from_rotation_matrix(x)[1], [x_init])
def test_from_rotation_matrix_random(self): """Tests rotation around Z axis.""" def get_rotation_matrix_around_z(angle_rad): return np.array([ [np.cos(angle_rad), -np.sin(angle_rad), 0], [np.sin(angle_rad), np.cos(angle_rad), 0], [0, 0, 1], ]) tensor_size = np.random.randint(10) angle = ( np.array([ np.deg2rad(np.random.randint(720) - 360) for _ in range(tensor_size) ]).reshape((tensor_size, 1))) rotation_matrix = [get_rotation_matrix_around_z(i[0]) for i in angle] rotation_matrix = np.array(rotation_matrix).reshape((tensor_size, 3, 3)) tf_axis, tf_angle = axis_angle.from_rotation_matrix(rotation_matrix) axis = np.tile([[0., 0., 1.]], (angle.shape[0], 1)) tf_quat_gt = quaternion.from_axis_angle(axis, angle) tf_quat = quaternion.from_axis_angle(tf_axis, tf_angle) # Compare quaternions since axis orientation and angle ambiguity will # lead to more complex comparisons. for quat_gt, quat in zip(self.evaluate(tf_quat_gt), self.evaluate(tf_quat)): # Remember that q=-q for any quaternion. pos = np.allclose(quat_gt, quat) neg = np.allclose(quat_gt, -quat) self.assertTrue(pos or neg)
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 half_rotation(rotation): """Return half of the input rotation. Args: rotation: [BATCH, 3, 3] rotation matrices. Returns: [BATCH, 3, 3] rotation matrices. """ axes, angles = axis_angle.from_rotation_matrix(rotation) return rotation_matrix_3d.from_axis_angle(axes, angles / 2)
def test_from_rotation_matrix_jacobian_random(self): """Test the Jacobian of the from_rotation_matrix function. Note: Preset angles are not tested as the gradient of tf.norm is NaN a 0. """ x_init = test_helpers.generate_random_test_rotation_matrix_3d() x = tf.convert_to_tensor(value=x_init) y_axis, y_angle = axis_angle.from_rotation_matrix(x) self.assert_jacobian_is_finite(x, x_init, y_axis) self.assert_jacobian_is_finite(x, x_init, y_angle)