# Path: sunds/core/tf_geometry/cameras.py
# class AbstractCamera(abc.ABC):
# class PinholeCamera(AbstractCamera):
#   def project(self, points: TensorLike) -> tf.Tensor:
#   def unproject(self,
#                 points: Optional[TensorLike] = None,
#                 to_rays: bool = True) -> tf.Tensor:
#   def pixel_centers(self) -> tf.Tensor:
#   def __post_init__(self):
#   def from_fov(
#       cls,
#       *,
#       image_width: int,
#       image_height: int,
#       horizontal_fov_in_degrees: float,
#       vertical_fov_in_degrees: Optional[float] = None) -> 'PinholeCamera':
#     def focal_length_from_fov(image_size, fov_in_degrees):
#   def from_intrinsics(
#       cls,
#       *,
#       image_size_in_pixels: Tuple[int, int],
#       focal_length_in_pixels: Tuple[float, float],
#       principal_point_in_pixels: Optional[Tuple[float, float]] = None,
#       skew: float = 0.0,
#   ) -> 'PinholeCamera':
#   def project(self, points: TensorLike) -> tf.Tensor:
#   def unproject(self,
#                 points: Optional[TensorLike] = None,
#                 to_rays: bool = True) -> tf.Tensor:
#   def pixel_centers(self) -> tf.Tensor:
#   K: TensorLike  # pylint: disable=invalid-name


import numpy as np
import tensorflow as tf
from sunds.core.tf_geometry import cameras
  def test_unproject_points_to_ray_directions(self):
    """Unproject a grid of 2D image points as rays and verify ray properties."""
    num_points = 10
    image_width, image_height = 640, 480
    points_image = np.column_stack(
        (np.random.uniform(0.0, image_width, num_points),
         np.random.uniform(0.0, image_height, num_points)))
    intrinsics = np.array([[500, 0., 320.], [0., 500., 120.], [0., 0., 1.]])
    camera_model = cameras.PinholeCamera(
        K=intrinsics, image_width=image_width, image_height=image_height)
    points_camera = camera_model.unproject(points_image, to_rays=True)
    self.assertEqual(points_camera.shape, (num_points, 3))

    # Assert thet each ray is a unit vector.
    self.assertAllClose(tf.norm(points_camera, axis=-1), tf.ones(num_points))

    # Assert thet each ray project backs to the same image coordinate.
    for point_camera, point_image in zip(points_camera, points_image):
      expected = intrinsics.dot(point_camera)
      expected = expected[:2] / expected[2]
      self.assertAllClose(point_image, expected)

  def test_unproject_points_to_unit_zplane(self):
    """Unproject a grid of 2D image points to z=1 plane and verify correctness."""
    num_points = 10
    image_width, image_height = 640, 480
    points_image = np.column_stack(
        (np.random.uniform(0.0, image_width, num_points),
         np.random.uniform(0.0, image_height, num_points)))
    intrinsics = np.array([[500, 0., 320.], [0., 500., 120.], [0., 0., 1.]])

# Path: sunds/core/tf_geometry/cameras.py
# class AbstractCamera(abc.ABC):
# class PinholeCamera(AbstractCamera):
#   def project(self, points: TensorLike) -> tf.Tensor:
#   def unproject(self,
#                 points: Optional[TensorLike] = None,
#                 to_rays: bool = True) -> tf.Tensor:
#   def pixel_centers(self) -> tf.Tensor:
#   def __post_init__(self):
#   def from_fov(
#       cls,
#       *,
#       image_width: int,
#       image_height: int,
#       horizontal_fov_in_degrees: float,
#       vertical_fov_in_degrees: Optional[float] = None) -> 'PinholeCamera':
#     def focal_length_from_fov(image_size, fov_in_degrees):
#   def from_intrinsics(
#       cls,
#       *,
#       image_size_in_pixels: Tuple[int, int],
#       focal_length_in_pixels: Tuple[float, float],
#       principal_point_in_pixels: Optional[Tuple[float, float]] = None,
#       skew: float = 0.0,
#   ) -> 'PinholeCamera':
#   def project(self, points: TensorLike) -> tf.Tensor:
#   def unproject(self,
#                 points: Optional[TensorLike] = None,
#                 to_rays: bool = True) -> tf.Tensor:
#   def pixel_centers(self) -> tf.Tensor:
#   K: TensorLike  # pylint: disable=invalid-name


# Path: sunds/core/tf_geometry/cameras_test.py
import numpy as np
import tensorflow as tf
from sunds.core.tf_geometry import cameras

  def test_unproject_points_to_ray_directions(self):
    """Unproject a grid of 2D image points as rays and verify ray properties."""
    num_points = 10
    image_width, image_height = 640, 480
    points_image = np.column_stack(
        (np.random.uniform(0.0, image_width, num_points),
         np.random.uniform(0.0, image_height, num_points)))
    intrinsics = np.array([[500, 0., 320.], [0., 500., 120.], [0., 0., 1.]])
    camera_model = cameras.PinholeCamera(
        K=intrinsics, image_width=image_width, image_height=image_height)
    points_camera = camera_model.unproject(points_image, to_rays=True)
    self.assertEqual(points_camera.shape, (num_points, 3))

    # Assert thet each ray is a unit vector.
    self.assertAllClose(tf.norm(points_camera, axis=-1), tf.ones(num_points))

    # Assert thet each ray project backs to the same image coordinate.
    for point_camera, point_image in zip(points_camera, points_image):
      expected = intrinsics.dot(point_camera)
      expected = expected[:2] / expected[2]
      self.assertAllClose(point_image, expected)

  def test_unproject_points_to_unit_zplane(self):
    """Unproject a grid of 2D image points to z=1 plane and verify correctness."""
    num_points = 10
    image_width, image_height = 640, 480
    points_image = np.column_stack(
        (np.random.uniform(0.0, image_width, num_points),
         np.random.uniform(0.0, image_height, num_points)))
    intrinsics = np.array([[500, 0., 320.], [0., 500., 120.], [0., 0., 1.]])

    camera_model = cameras.PinholeCamera(