Adds function to define camera configs through intrinsic matrix (#617)

# Description

This PR adds the possibility of initializing cameras (both Raycaster
Cameras and USD Cameras) with the intrinsic matrix instead of using the
aperture parameters. The intrinsic matrix is defined in the pattern
config and the pinhole cameras spawn config, respectively.

Moreover, it fixes the bug that the vertical aperture is not adjusted
for the USD camera case (it will always stay at the default value, even
if the horizontal aperture is set). The default is squared pixels;
however, it also allows the setting of other values.

Fixes https://github.com/isaac-orbit/IsaacLab/issues/226

## Type of change

- New feature (non-breaking change which adds functionality)

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.sh --format`
- [ ] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [x] I have added tests that prove my fix is effective or that my
feature works
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [x] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there

source/extensions/omni.isaac.lab/config/extension.toml

 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.22.2"
+version = "0.22.3"
 
 # Description
 title = "Isaac Lab framework for Robot Learning"

source/extensions/omni.isaac.lab/docs/CHANGELOG.rst

 Changelog
 ---------
 
+0.22.3 (2024-08-28)
+~~~~~~~~~~~~~~~~~~~
+
+Added
+^^^^^
+
+* Added a class method to initialize camera configurations with an intrinsic matrix in the
+  :class:`omni.isaac.lab.sim.spawner.sensors.PinholeCameraCfg`
+  :class:`omni.isaac.lab.sensors.ray_caster.patterns_cfg.PinholeCameraPatternCfg` classes.
+
+Fixed
+^^^^^
+
+* Fixed the ray direction in :func:`omni.isaac.lab.sensors.ray_caster.patterns.patterns.pinhole_camera_pattern` to
+  point to the center of the pixel instead of the top-left corner.
+* Fixed the clipping of the "distance_to_image_plane" depth image obtained using the
+  :class:`omni.isaac.lab.sensors.ray_caster.RayCasterCamera` class. Earlier, the depth image was being clipped
+  before the depth image was generated. Now, the clipping is applied after the depth image is generated. This makes
+  the behavior equal to the USD Camera.
+
+
 0.22.2 (2024-08-21)
 ~~~~~~~~~~~~~~~~~~~
 

source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py

@@ -5,7 +5,6 @@
 
 from __future__ import annotations
 
-import math
 import numpy as np
 import re
 import torch
@@ -117,6 +116,9 @@ class Camera(SensorBase):
             rot = torch.tensor(self.cfg.offset.rot, dtype=torch.float32).unsqueeze(0)
             rot_offset = convert_orientation_convention(rot, origin=self.cfg.offset.convention, target="opengl")
             rot_offset = rot_offset.squeeze(0).numpy()
+            # ensure vertical aperture is set, otherwise replace with default for squared pixels
+            if self.cfg.spawn.vertical_aperture is None:
+                self.cfg.spawn.vertical_aperture = self.cfg.spawn.horizontal_aperture * self.cfg.height / self.cfg.width
             # spawn the asset
             self.cfg.spawn.func(
                 self.cfg.prim_path, self.cfg.spawn, translation=self.cfg.offset.pos, orientation=rot_offset
@@ -243,6 +245,12 @@ class Camera(SensorBase):
                 "horizontal_aperture_offset": (c_x - width / 2) / f_x,
                 "vertical_aperture_offset": (c_y - height / 2) / f_y,
             }
+
+            # TODO: Adjust to handle aperture offsets once supported by omniverse
+            #   Internal ticket from rendering team: OM-42611
+            if params["horizontal_aperture_offset"] > 1e-4 or params["vertical_aperture_offset"] > 1e-4:
+                carb.log_warn("Camera aperture offsets are not supported by Omniverse. These parameters are ignored.")
+
             # change data for corresponding camera index
             sensor_prim = self._sensor_prims[i]
             # set parameters for camera
@@ -541,17 +549,21 @@ class Camera(SensorBase):
             # get camera parameters
             focal_length = sensor_prim.GetFocalLengthAttr().Get()
             horiz_aperture = sensor_prim.GetHorizontalApertureAttr().Get()
+            vert_aperture = sensor_prim.GetVerticalApertureAttr().Get()
+            horiz_aperture_offset = sensor_prim.GetHorizontalApertureOffsetAttr().Get()
+            vert_aperture_offset = sensor_prim.GetVerticalApertureOffsetAttr().Get()
             # get viewport parameters
             height, width = self.image_shape
-            # calculate the field of view
-            fov = 2 * math.atan(horiz_aperture / (2 * focal_length))
-            # calculate the focal length in pixels
-            focal_px = width * 0.5 / math.tan(fov / 2)
+            # extract intrinsic parameters
+            f_x = (width * focal_length) / horiz_aperture
+            f_y = (height * focal_length) / vert_aperture
+            c_x = width * 0.5 + horiz_aperture_offset * f_x
+            c_y = height * 0.5 + vert_aperture_offset * f_y
             # create intrinsic matrix for depth linear
-            self._data.intrinsic_matrices[i, 0, 0] = focal_px
-            self._data.intrinsic_matrices[i, 0, 2] = width * 0.5
-            self._data.intrinsic_matrices[i, 1, 1] = focal_px
-            self._data.intrinsic_matrices[i, 1, 2] = height * 0.5
+            self._data.intrinsic_matrices[i, 0, 0] = f_x
+            self._data.intrinsic_matrices[i, 0, 2] = c_x
+            self._data.intrinsic_matrices[i, 1, 1] = f_y
+            self._data.intrinsic_matrices[i, 1, 2] = c_y
             self._data.intrinsic_matrices[i, 2, 2] = 1
 
     def _update_poses(self, env_ids: Sequence[int]):

source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/ray_caster/patterns/patterns.py

@@ -86,6 +86,8 @@ def pinhole_camera_pattern(
     pixels = torch.vstack(list(map(torch.ravel, grid))).T
     # convert to homogeneous coordinate system
     pixels = torch.hstack([pixels, torch.ones((len(pixels), 1), device=device)])
+    # move each pixel coordinate to the center of the pixel
+    pixels += torch.tensor([[0.5, 0.5, 0]], device=device)
     # get pixel coordinates in camera frame
     pix_in_cam_frame = torch.matmul(torch.inverse(intrinsic_matrices), pixels.T)
 

source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/ray_caster/patterns/patterns_cfg.py

@@ -79,6 +79,7 @@ class PinholeCameraPatternCfg(PatternBaseCfg):
 
     Longer lens lengths narrower FOV, shorter lens lengths wider FOV.
     """
+
     horizontal_aperture: float = 20.955
     """Horizontal aperture (in mm). Defaults to 20.955mm.
 
@@ -87,15 +88,84 @@ class PinholeCameraPatternCfg(PatternBaseCfg):
     Note:
         The default value is the horizontal aperture of a 35 mm spherical projector.
     """
+
+    vertical_aperture: float | None = None
+    """Vertical aperture (in mm). Defaults to None.
+
+    Emulates sensor/film height on a camera. If None, then the vertical aperture is calculated based on the
+    horizontal aperture and the aspect ratio of the image to maintain squared pixels. In this case, the vertical
+    aperture is calculated as:
+
+    .. math::
+        \text{vertical aperture} = \text{horizontal aperture} \times \frac{\text{height}}{\text{width}}
+    """
+
     horizontal_aperture_offset: float = 0.0
     """Offsets Resolution/Film gate horizontally. Defaults to 0.0."""
+
     vertical_aperture_offset: float = 0.0
     """Offsets Resolution/Film gate vertically. Defaults to 0.0."""
+
     width: int = MISSING
     """Width of the image (in pixels)."""
+
     height: int = MISSING
     """Height of the image (in pixels)."""
 
+    @classmethod
+    def from_intrinsic_matrix(
+        cls,
+        intrinsic_matrix: list[float],
+        width: int,
+        height: int,
+        focal_length: float = 24.0,
+    ) -> PinholeCameraPatternCfg:
+        r"""Create a :class:`PinholeCameraPatternCfg` class instance from an intrinsic matrix.
+
+        The intrinsic matrix is a 3x3 matrix that defines the mapping between the 3D world coordinates and
+        the 2D image. The matrix is defined as:
+
+        .. math::
+            I_{cam} = \begin{bmatrix}
+            f_x & 0 & c_x \\
+            0 & f_y & c_y \\
+            0 & 0 & 1
+            \end{bmatrix},
+
+        where :math:`f_x` and :math:`f_y` are the focal length along x and y direction, while :math:`c_x` and :math:`c_y` are the
+        principle point offsets along x and y direction respectively.
+
+        Args:
+            intrinsic_matrix: Intrinsic matrix of the camera in row-major format.
+                The matrix is defined as [f_x, 0, c_x, 0, f_y, c_y, 0, 0, 1]. Shape is (9,).
+            width: Width of the image (in pixels).
+            height: Height of the image (in pixels).
+            focal_length: Focal length of the camera (in cm). Defaults to 24.0 cm.
+
+        Returns:
+            An instance of the :class:`PinholeCameraPatternCfg` class.
+        """
+        # extract parameters from matrix
+        f_x = intrinsic_matrix[0]
+        c_x = intrinsic_matrix[2]
+        f_y = intrinsic_matrix[4]
+        c_y = intrinsic_matrix[5]
+        # resolve parameters for usd camera
+        horizontal_aperture = width * focal_length / f_x
+        vertical_aperture = height * focal_length / f_y
+        horizontal_aperture_offset = (c_x - width / 2) / f_x
+        vertical_aperture_offset = (c_y - height / 2) / f_y
+
+        return cls(
+            focal_length=focal_length,
+            horizontal_aperture=horizontal_aperture,
+            vertical_aperture=vertical_aperture,
+            horizontal_aperture_offset=horizontal_aperture_offset,
+            vertical_aperture_offset=vertical_aperture_offset,
+            width=width,
+            height=height,
+        )
+
 
 @configclass
 class BpearlPatternCfg(PatternBaseCfg):

source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/ray_caster/ray_caster_camera.py

@@ -264,14 +264,19 @@ class RayCasterCamera(RayCaster):
         ray_starts_w = math_utils.quat_apply(quat_w.repeat(1, self.num_rays), self.ray_starts[env_ids])
         ray_starts_w += pos_w.unsqueeze(1)
         ray_directions_w = math_utils.quat_apply(quat_w.repeat(1, self.num_rays), self.ray_directions[env_ids])
+
         # ray cast and store the hits
+        # note: we set max distance to 1e6 during the ray-casting. THis is because we clip the distance
+        # to the image plane and distance to the camera to the maximum distance afterwards in-order to
+        # match the USD camera behavior.
+
         # TODO: Make ray-casting work for multiple meshes?
         # necessary for regular dictionaries.
         self.ray_hits_w, ray_depth, ray_normal, _ = raycast_mesh(
             ray_starts_w,
             ray_directions_w,
             mesh=RayCasterCamera.meshes[self.cfg.mesh_prim_paths[0]],
-            max_dist=self.cfg.max_distance,
+            max_dist=1e6,
             return_distance=any(
                 [name in self.cfg.data_types for name in ["distance_to_image_plane", "distance_to_camera"]]
             ),
@@ -286,9 +291,13 @@ class RayCasterCamera(RayCaster):
                     (ray_depth[:, :, None] * ray_directions_w),
                 )
             )[:, :, 0]
+            # apply the maximum distance after the transformation
+            distance_to_image_plane = torch.clip(distance_to_image_plane, max=self.cfg.max_distance)
             self._data.output["distance_to_image_plane"][env_ids] = distance_to_image_plane.view(-1, *self.image_shape)
         if "distance_to_camera" in self.cfg.data_types:
-            self._data.output["distance_to_camera"][env_ids] = ray_depth.view(-1, *self.image_shape)
+            self._data.output["distance_to_camera"][env_ids] = torch.clip(
+                ray_depth.view(-1, *self.image_shape), max=self.cfg.max_distance
+            )
         if "normals" in self.cfg.data_types:
             self._data.output["normals"][env_ids] = ray_normal.view(-1, *self.image_shape, 3)
 
@@ -346,10 +355,15 @@ class RayCasterCamera(RayCaster):
         """Computes the intrinsic matrices for the camera based on the config provided."""
         # get the sensor properties
         pattern_cfg = self.cfg.pattern_cfg
+
+        # check if vertical aperture is provided
+        # if not then it is auto-computed based on the aspect ratio to preserve squared pixels
+        if pattern_cfg.vertical_aperture is None:
+            pattern_cfg.vertical_aperture = pattern_cfg.horizontal_aperture * pattern_cfg.height / pattern_cfg.width
+
         # compute the intrinsic matrix
-        vertical_aperture = pattern_cfg.horizontal_aperture * pattern_cfg.height / pattern_cfg.width
         f_x = pattern_cfg.width * pattern_cfg.focal_length / pattern_cfg.horizontal_aperture
-        f_y = pattern_cfg.height * pattern_cfg.focal_length / vertical_aperture
+        f_y = pattern_cfg.height * pattern_cfg.focal_length / pattern_cfg.vertical_aperture
         c_x = pattern_cfg.horizontal_aperture_offset * f_x + pattern_cfg.width / 2
         c_y = pattern_cfg.vertical_aperture_offset * f_y + pattern_cfg.height / 2
         # allocate the intrinsic matrices
@@ -357,6 +371,7 @@ class RayCasterCamera(RayCaster):
         self._data.intrinsic_matrices[:, 0, 2] = c_x
         self._data.intrinsic_matrices[:, 1, 1] = f_y
         self._data.intrinsic_matrices[:, 1, 2] = c_y
+
         # save focal length
         self._focal_length = pattern_cfg.focal_length
 

source/extensions/omni.isaac.lab/omni/isaac/lab/sim/spawners/sensors/sensors.py

@@ -7,6 +7,7 @@ from __future__ import annotations
 
 from typing import TYPE_CHECKING
 
+import carb
 import omni.isaac.core.utils.prims as prim_utils
 import omni.kit.commands
 from pxr import Sdf, Usd
@@ -99,9 +100,21 @@ def spawn_camera(
         attribute_types = CUSTOM_PINHOLE_CAMERA_ATTRIBUTES
     else:
         attribute_types = CUSTOM_FISHEYE_CAMERA_ATTRIBUTES
-    # custom attributes in the config that are not USD Camera parameters
-    non_usd_cfg_param_names = ["func", "copy_from_source", "lock_camera", "visible", "semantic_tags"]
 
+    # TODO: Adjust to handle aperture offsets once supported by omniverse
+    #   Internal ticket from rendering team: OM-42611
+    if cfg.horizontal_aperture_offset > 1e-4 or cfg.vertical_aperture_offset > 1e-4:
+        carb.log_warn("Camera aperture offsets are not supported by Omniverse. These parameters will be ignored.")
+
+    # custom attributes in the config that are not USD Camera parameters
+    non_usd_cfg_param_names = [
+        "func",
+        "copy_from_source",
+        "lock_camera",
+        "visible",
+        "semantic_tags",
+        "from_intrinsic_matrix",
+    ]
     # get camera prim
     prim = prim_utils.get_prim_at_path(prim_path)
     # create attributes for the fisheye camera model

source/extensions/omni.isaac.lab/omni/isaac/lab/sim/spawners/sensors/sensors_cfg.py

@@ -3,6 +3,8 @@
 #
 # SPDX-License-Identifier: BSD-3-Clause
 
+from __future__ import annotations
+
 from collections.abc import Callable
 from typing import Literal
 
@@ -31,27 +33,32 @@ class PinholeCameraCfg(SpawnerCfg):
     Note:
         Currently only "pinhole" is supported.
     """
+
     clipping_range: tuple[float, float] = (0.01, 1e6)
     """Near and far clipping distances (in m). Defaults to (0.01, 1e6).
 
     The minimum clipping range will shift the camera forward by the specified distance. Don't set it too high to
     avoid issues for distance related data types (e.g., ``distance_to_image_plane``).
     """
+
     focal_length: float = 24.0
     """Perspective focal length (in cm). Defaults to 24.0cm.
 
     Longer lens lengths narrower FOV, shorter lens lengths wider FOV.
     """
+
     focus_distance: float = 400.0
     """Distance from the camera to the focus plane (in m). Defaults to 400.0.
 
     The distance at which perfect sharpness is achieved.
     """
+
     f_stop: float = 0.0
     """Lens aperture. Defaults to 0.0, which turns off focusing.
 
     Controls Distance Blurring. Lower Numbers decrease focus range, larger numbers increase it.
     """
+
     horizontal_aperture: float = 20.955
     """Horizontal aperture (in mm). Defaults to 20.955mm.
 
@@ -60,10 +67,23 @@ class PinholeCameraCfg(SpawnerCfg):
     Note:
         The default value is the horizontal aperture of a 35 mm spherical projector.
     """
+
+    vertical_aperture: float | None = None
+    r"""Vertical aperture (in mm). Defaults to None.
+
+    Emulates sensor/film height on a camera. If None, then the vertical aperture is calculated based on the
+    horizontal aperture and the aspect ratio of the image to maintain squared pixels. This is calculated as:
+
+    .. math::
+        \text{vertical aperture} = \text{horizontal aperture} \times \frac{\text{height}}{\text{width}}
+    """
+
     horizontal_aperture_offset: float = 0.0
     """Offsets Resolution/Film gate horizontally. Defaults to 0.0."""
+
     vertical_aperture_offset: float = 0.0
     """Offsets Resolution/Film gate vertically. Defaults to 0.0."""
+
     lock_camera: bool = True
     """Locks the camera in the Omniverse viewport. Defaults to True.
 
@@ -71,6 +91,77 @@ class PinholeCameraCfg(SpawnerCfg):
     the camera output on the GUI and not accidentally moving the camera through the GUI interactions.
     """
 
+    @classmethod
+    def from_intrinsic_matrix(
+        cls,
+        intrinsic_matrix: list[float],
+        width: int,
+        height: int,
+        clipping_range: tuple[float, float] = (0.01, 1e6),
+        focal_length: float = 24.0,
+        focus_distance: float = 400.0,
+        f_stop: float = 0.0,
+        projection_type: str = "pinhole",
+        lock_camera: bool = True,
+    ) -> PinholeCameraCfg:
+        r"""Create a :class:`PinholeCameraCfg` class instance from an intrinsic matrix.
+
+        The intrinsic matrix is a 3x3 matrix that defines the mapping between the 3D world coordinates and
+        the 2D image. The matrix is defined as:
+
+        .. math::
+            I_{cam} = \begin{bmatrix}
+            f_x & 0 & c_x \\
+            0 & f_y & c_y \\
+            0 & 0 & 1
+            \\end{bmatrix},
+
+        where :math:`f_x` and :math:`f_y` are the focal length along x and y direction, while :math:`c_x` and :math:`c_y` are the
+        principle point offsets along x and y direction respectively.
+
+        Args:
+            intrinsic_matrix: Intrinsic matrix of the camera in row-major format.
+                The matrix is defined as [f_x, 0, c_x, 0, f_y, c_y, 0, 0, 1]. Shape is (9,).
+            width: Width of the image (in pixels).
+            height: Height of the image (in pixels).
+            clipping_range: Near and far clipping distances (in m). Defaults to (0.01, 1e6).
+            focal_length: Perspective focal length (in cm). Defaults to 24.0 cm.
+            focus_distance: Distance from the camera to the focus plane (in m). Defaults to 400.0 m.
+            f_stop: Lens aperture. Defaults to 0.0, which turns off focusing.
+            projection_type: Type of projection to use for the camera. Defaults to "pinhole".
+            lock_camera: Locks the camera in the Omniverse viewport. Defaults to True.
+
+        Returns:
+            An instance of the :class:`PinholeCameraCfg` class.
+        """
+        # raise not implemented error is projection type is not pinhole
+        if projection_type != "pinhole":
+            raise NotImplementedError("Only pinhole projection type is supported.")
+
+        # extract parameters from matrix
+        f_x = intrinsic_matrix[0]
+        c_x = intrinsic_matrix[2]
+        f_y = intrinsic_matrix[4]
+        c_y = intrinsic_matrix[5]
+        # resolve parameters for usd camera
+        horizontal_aperture = width * focal_length / f_x
+        vertical_aperture = height * focal_length / f_y
+        horizontal_aperture_offset = (c_x - width / 2) / f_x
+        vertical_aperture_offset = (c_y - height / 2) / f_y
+
+        return cls(
+            projection_type=projection_type,
+            clipping_range=clipping_range,
+            focal_length=focal_length,
+            focus_distance=focus_distance,
+            f_stop=f_stop,
+            horizontal_aperture=horizontal_aperture,
+            vertical_aperture=vertical_aperture,
+            horizontal_aperture_offset=horizontal_aperture_offset,
+            vertical_aperture_offset=vertical_aperture_offset,
+            lock_camera=lock_camera,
+        )
+
 
 @configclass
 class FisheyeCameraCfg(PinholeCameraCfg):
@@ -101,25 +192,36 @@ class FisheyeCameraCfg(PinholeCameraCfg):
     - ``"fisheye_polynomial"``: Fisheye camera model with :math:`360^{\circ}` spherical projection.
     - ``"fisheye_spherical"``: Fisheye camera model with :math:`360^{\circ}` full-frame projection.
     """
+
     fisheye_nominal_width: float = 1936.0
     """Nominal width of fisheye lens model (in pixels). Defaults to 1936.0."""
+
     fisheye_nominal_height: float = 1216.0
     """Nominal height of fisheye lens model (in pixels). Defaults to 1216.0."""
+
     fisheye_optical_centre_x: float = 970.94244
     """Horizontal optical centre position of fisheye lens model (in pixels). Defaults to 970.94244."""
+
     fisheye_optical_centre_y: float = 600.37482
     """Vertical optical centre position of fisheye lens model (in pixels). Defaults to 600.37482."""
+
     fisheye_max_fov: float = 200.0
     """Maximum field of view of fisheye lens model (in degrees). Defaults to 200.0 degrees."""
+
     fisheye_polynomial_a: float = 0.0
     """First component of fisheye polynomial. Defaults to 0.0."""
+
     fisheye_polynomial_b: float = 0.00245
     """Second component of fisheye polynomial. Defaults to 0.00245."""
+
     fisheye_polynomial_c: float = 0.0
     """Third component of fisheye polynomial. Defaults to 0.0."""
+
     fisheye_polynomial_d: float = 0.0
     """Fourth component of fisheye polynomial. Defaults to 0.0."""
+
     fisheye_polynomial_e: float = 0.0
     """Fifth component of fisheye polynomial. Defaults to 0.0."""
+
     fisheye_polynomial_f: float = 0.0
     """Sixth component of fisheye polynomial. Defaults to 0.0."""

source/extensions/omni.isaac.lab/test/sensors/test_camera.py

@@ -231,6 +231,57 @@ class TestCamera(unittest.TestCase):
                 for im_data in cam.data.output.to_dict().values():
                     self.assertEqual(im_data.shape, (1, self.camera_cfg.height, self.camera_cfg.width))
 
+    def test_camera_init_intrinsic_matrix(self):
+        """Test camera initialization from intrinsic matrix."""
+        # get the first camera
+        camera_1 = Camera(cfg=self.camera_cfg)
+        # get intrinsic matrix
+        self.sim.reset()
+        intrinsic_matrix = camera_1.data.intrinsic_matrices[0].cpu().flatten().tolist()
+        self.tearDown()
+        # reinit the first camera
+        self.setUp()
+        camera_1 = Camera(cfg=self.camera_cfg)
+        # initialize from intrinsic matrix
+        intrinsic_camera_cfg = CameraCfg(
+            height=128,
+            width=128,
+            prim_path="/World/Camera_2",
+            update_period=0,
+            data_types=["distance_to_image_plane"],
+            spawn=sim_utils.PinholeCameraCfg().from_intrinsic_matrix(
+                intrinsic_matrix=intrinsic_matrix,
+                width=128,
+                height=128,
+                focal_length=24.0,
+                focus_distance=400.0,
+                clipping_range=(0.1, 1.0e5),
+            ),
+        )
+        camera_2 = Camera(cfg=intrinsic_camera_cfg)
+
+        # play sim
+        self.sim.reset()
+
+        # update cameras
+        camera_1.update(self.dt)
+        camera_2.update(self.dt)
+
+        # check image data
+        torch.testing.assert_close(
+            camera_1.data.output["distance_to_image_plane"],
+            camera_2.data.output["distance_to_image_plane"],
+            rtol=5e-3,
+            atol=1e-4,
+        )
+        # check that both intrinsic matrices are the same
+        torch.testing.assert_close(
+            camera_1.data.intrinsic_matrices[0],
+            camera_2.data.intrinsic_matrices[0],
+            rtol=5e-3,
+            atol=1e-4,
+        )
+
     def test_camera_set_world_poses(self):
         """Test camera function to set specific world pose."""
         camera = Camera(self.camera_cfg)

source/extensions/omni.isaac.lab/test/sensors/test_ray_caster_camera.py

@@ -79,7 +79,7 @@ class TestWarpCamera(unittest.TestCase):
         sim_cfg = sim_utils.SimulationCfg(dt=self.dt)
         self.sim: sim_utils.SimulationContext = sim_utils.SimulationContext(sim_cfg)
         # Ground-plane
-        mesh = make_plane(size=(2e1, 2e1), height=0.0, center_zero=True)
+        mesh = make_plane(size=(100, 100), height=0.0, center_zero=True)
         create_prim_from_mesh("/World/defaultGroundPlane", mesh)
         # load stage
         stage_utils.update_stage()
@@ -203,6 +203,55 @@ class TestWarpCamera(unittest.TestCase):
         np.testing.assert_allclose(camera_ros.data.quat_w_opengl[0].cpu().numpy(), QUAT_OPENGL, rtol=1e-5)
         np.testing.assert_allclose(camera_ros.data.quat_w_world[0].cpu().numpy(), QUAT_WORLD, rtol=1e-5)
 
+    def test_camera_init_intrinsic_matrix(self):
+        """Test camera initialization from intrinsic matrix."""
+        # get the first camera
+        camera_1 = RayCasterCamera(cfg=self.camera_cfg)
+        # get intrinsic matrix
+        self.sim.reset()
+        intrinsic_matrix = camera_1.data.intrinsic_matrices[0].cpu().flatten().tolist()
+        self.tearDown()
+        # reinit the first camera
+        self.setUp()
+        camera_1 = RayCasterCamera(cfg=self.camera_cfg)
+        # initialize from intrinsic matrix
+        intrinsic_camera_cfg = RayCasterCameraCfg(
+            prim_path="/World/Camera",
+            mesh_prim_paths=["/World/defaultGroundPlane"],
+            update_period=0,
+            offset=RayCasterCameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(1.0, 0.0, 0.0, 0.0), convention="world"),
+            debug_vis=False,
+            pattern_cfg=patterns.PinholeCameraPatternCfg().from_intrinsic_matrix(
+                intrinsic_matrix=intrinsic_matrix,
+                height=self.camera_cfg.pattern_cfg.height,
+                width=self.camera_cfg.pattern_cfg.width,
+                focal_length=self.camera_cfg.pattern_cfg.focal_length,
+            ),
+            data_types=[
+                "distance_to_image_plane",
+            ],
+        )
+        camera_2 = RayCasterCamera(cfg=intrinsic_camera_cfg)
+
+        # play sim
+        self.sim.reset()
+        self.sim.play()
+
+        # update cameras
+        camera_1.update(self.dt)
+        camera_2.update(self.dt)
+
+        # check image data
+        torch.testing.assert_close(
+            camera_1.data.output["distance_to_image_plane"],
+            camera_2.data.output["distance_to_image_plane"],
+        )
+        # check that both intrinsic matrices are the same
+        torch.testing.assert_close(
+            camera_1.data.intrinsic_matrices[0],
+            camera_2.data.intrinsic_matrices[0],
+        )
+
     def test_multi_camera_init(self):
         """Test multi-camera initialization."""
         # create two cameras with different prim paths
@@ -301,11 +350,7 @@ class TestWarpCamera(unittest.TestCase):
             # update camera
             camera.update(self.dt)
             # Check that matrix is correct
-            # TODO: This is not correctly setting all values in the matrix since the
-            #       vertical aperture and aperture offsets are not being set correctly
-            #       This is a bug in the simulator.
-            torch.testing.assert_close(rs_intrinsic_matrix[0, 0, 0], camera.data.intrinsic_matrices[0, 0, 0])
-            # torch.testing.assert_close(rs_intrinsic_matrix[0, 1, 1], camera.data.intrinsic_matrices[0, 1, 1])
+            torch.testing.assert_close(rs_intrinsic_matrix, camera.data.intrinsic_matrices)
 
     def test_throughput(self):
         """Checks that the single camera gets created properly with a rig."""
@@ -411,19 +456,36 @@ class TestWarpCamera(unittest.TestCase):
         camera_usd.update(self.dt)
         camera_warp.update(self.dt)
 
+        # check the intrinsic matrices
+        torch.testing.assert_close(
+            camera_usd.data.intrinsic_matrices,
+            camera_warp.data.intrinsic_matrices,
+        )
+
+        # check the apertures
+        torch.testing.assert_close(
+            camera_usd._sensor_prims[0].GetHorizontalApertureAttr().Get(),
+            camera_cfg_warp.pattern_cfg.horizontal_aperture,
+        )
+        torch.testing.assert_close(
+            camera_usd._sensor_prims[0].GetVerticalApertureAttr().Get(),
+            camera_cfg_warp.pattern_cfg.vertical_aperture,
+        )
+
         # check image data
         torch.testing.assert_close(
             camera_usd.data.output["distance_to_image_plane"],
             camera_warp.data.output["distance_to_image_plane"],
-            rtol=5e-3,
-            atol=1e-4,
         )
         torch.testing.assert_close(
             camera_usd.data.output["distance_to_camera"],
             camera_warp.data.output["distance_to_camera"],
-            rtol=5e-3,
-            atol=1e-4,
+            atol=5e-5,
+            rtol=5e-6,
         )
+
+        # check normals
+        # NOTE: floating point issues of ~1e-5, so using atol and rtol in this case
         torch.testing.assert_close(
             camera_usd.data.output["normals"][..., :3],
             camera_warp.data.output["normals"],
@@ -483,15 +545,14 @@ class TestWarpCamera(unittest.TestCase):
         torch.testing.assert_close(
             camera_usd.data.output["distance_to_image_plane"],
             camera_warp.data.output["distance_to_image_plane"],
-            rtol=5e-3,
-            atol=1e-4,
         )
         torch.testing.assert_close(
             camera_usd.data.output["distance_to_camera"],
             camera_warp.data.output["distance_to_camera"],
-            rtol=5e-3,
-            atol=1e-4,
         )
+
+        # check normals
+        # NOTE: floating point issues of ~1e-5, so using atol and rtol in this case
         torch.testing.assert_close(
             camera_usd.data.output["normals"][..., :3],
             camera_warp.data.output["normals"],
@@ -570,15 +631,16 @@ class TestWarpCamera(unittest.TestCase):
         torch.testing.assert_close(
             camera_usd.data.output["distance_to_image_plane"],
             camera_warp.data.output["distance_to_image_plane"],
-            rtol=5e-3,
-            atol=1e-4,
         )
         torch.testing.assert_close(
             camera_usd.data.output["distance_to_camera"],
             camera_warp.data.output["distance_to_camera"],
-            rtol=5e-3,
-            atol=1e-4,
+            rtol=4e-6,
+            atol=2e-5,
         )
+
+        # check normals
+        # NOTE: floating point issues of ~1e-5, so using atol and rtol in this case
         torch.testing.assert_close(
             camera_usd.data.output["normals"][..., :3],
             camera_warp.data.output["normals"],
@@ -586,6 +648,97 @@ class TestWarpCamera(unittest.TestCase):
             atol=1e-4,
         )
 
+    def test_output_equal_to_usd_camera_intrinsics(self):
+        """
+        Test that the output of the ray caster camera and usd camera are the same when both are
+        initialized with the same intrinsic matrix.
+        """
+
+        # create cameras
+        offset_rot = [-0.1251, 0.3617, 0.8731, -0.3020]
+        offset_pos = (2.5, 2.5, 4.0)
+        intrinsics = [380.0831, 0.0, 467.7916, 0.0, 380.0831, 262.0532, 0.0, 0.0, 1.0]
+        prim_utils.create_prim("/World/Camera_warp", "Xform")
+        # get camera cfgs
+        camera_warp_cfg = RayCasterCameraCfg(
+            prim_path="/World/Camera_warp",
+            mesh_prim_paths=["/World/defaultGroundPlane"],
+            offset=RayCasterCameraCfg.OffsetCfg(pos=offset_pos, rot=offset_rot, convention="ros"),
+            debug_vis=False,
+            pattern_cfg=patterns.PinholeCameraPatternCfg().from_intrinsic_matrix(
+                intrinsic_matrix=intrinsics,
+                height=540,
+                width=960,
+                focal_length=38.0,
+            ),
+            max_distance=20.0,
+            data_types=["distance_to_image_plane"],
+        )
+        camera_usd_cfg = CameraCfg(
+            prim_path="/World/Camera_usd",
+            offset=CameraCfg.OffsetCfg(pos=offset_pos, rot=offset_rot, convention="ros"),
+            spawn=PinholeCameraCfg().from_intrinsic_matrix(
+                intrinsic_matrix=intrinsics,
+                height=540,
+                width=960,
+                clipping_range=(0.01, 20),
+                focal_length=38.0,
+            ),
+            height=540,
+            width=960,
+            data_types=["distance_to_image_plane"],
+        )
+
+        # set aperture offsets to 0, as currently not supported for usd camera
+        camera_warp_cfg.pattern_cfg.horizontal_aperture_offset = 0
+        camera_warp_cfg.pattern_cfg.vertical_aperture_offset = 0
+        camera_usd_cfg.spawn.horizontal_aperture_offset = 0
+        camera_usd_cfg.spawn.vertical_aperture_offset = 0
+        # init cameras
+        camera_warp = RayCasterCamera(camera_warp_cfg)
+        camera_usd = Camera(camera_usd_cfg)
+
+        # play sim
+        self.sim.reset()
+        self.sim.play()
+
+        # perform steps
+        for _ in range(5):
+            self.sim.step()
+
+        # update camera
+        camera_usd.update(self.dt)
+        camera_warp.update(self.dt)
+
+        # filter nan and inf from output
+        cam_warp_output = camera_warp.data.output["distance_to_image_plane"].clone()
+        cam_usd_output = camera_usd.data.output["distance_to_image_plane"].clone()
+        cam_warp_output[torch.isnan(cam_warp_output)] = 0
+        cam_warp_output[torch.isinf(cam_warp_output)] = 0
+        cam_usd_output[torch.isnan(cam_usd_output)] = 0
+        cam_usd_output[torch.isinf(cam_usd_output)] = 0
+
+        # check that both have the same intrinsic matrices
+        torch.testing.assert_close(camera_warp.data.intrinsic_matrices[0], camera_usd.data.intrinsic_matrices[0])
+
+        # check the apertures
+        torch.testing.assert_close(
+            camera_usd._sensor_prims[0].GetHorizontalApertureAttr().Get(),
+            camera_warp_cfg.pattern_cfg.horizontal_aperture,
+        )
+        torch.testing.assert_close(
+            camera_usd._sensor_prims[0].GetVerticalApertureAttr().Get(),
+            camera_warp_cfg.pattern_cfg.vertical_aperture,
+        )
+
+        # check image data
+        torch.testing.assert_close(
+            cam_warp_output,
+            cam_usd_output,
+            atol=5e-5,
+            rtol=5e-6,
+        )
+
 
 if __name__ == "__main__":
     run_tests()

source/extensions/omni.isaac.lab/test/sensors/test_tiled_camera.py

@@ -19,6 +19,7 @@ simulation_app = app_launcher.app
 import copy
 import numpy as np
 import random
+import torch
 import unittest
 
 import omni.isaac.core.utils.prims as prim_utils
@@ -28,7 +29,7 @@ from omni.isaac.core.prims import GeometryPrim, RigidPrim
 from pxr import Gf, UsdGeom
 
 import omni.isaac.lab.sim as sim_utils
-from omni.isaac.lab.sensors.camera import TiledCamera, TiledCameraCfg
+from omni.isaac.lab.sensors.camera import Camera, CameraCfg, TiledCamera, TiledCameraCfg
 from omni.isaac.lab.utils.timer import Timer
 
 
@@ -299,6 +300,107 @@ class TestTiledCamera(unittest.TestCase):
                 self.assertGreater(im_data.mean().item(), 0.0)
         del camera
 
+    def test_output_equal_to_usd_camera_intrinsics(self):
+        """
+        Test that the output of the ray caster camera and the usd camera are the same when both are
+        initialized with the same intrinsic matrix.
+        """
+
+        # create cameras
+        offset_rot = (-0.1251, 0.3617, 0.8731, -0.3020)
+        offset_pos = (2.5, 2.5, 4.0)
+        intrinsics = [380.08, 0.0, 467.79, 0.0, 380.08, 262.05, 0.0, 0.0, 1.0]
+        # get camera cfgs
+        # TODO: add clipping range back, once correctly supported by tiled camera
+        camera_tiled_cfg = TiledCameraCfg(
+            prim_path="/World/Camera_tiled",
+            offset=TiledCameraCfg.OffsetCfg(pos=offset_pos, rot=offset_rot, convention="ros"),
+            spawn=sim_utils.PinholeCameraCfg().from_intrinsic_matrix(
+                intrinsic_matrix=intrinsics,
+                height=540,
+                width=960,
+                focal_length=38.0,
+                # clipping_range=(0.01, 20),
+            ),
+            height=540,
+            width=960,
+            data_types=["depth"],
+        )
+        camera_usd_cfg = CameraCfg(
+            prim_path="/World/Camera_usd",
+            offset=CameraCfg.OffsetCfg(pos=offset_pos, rot=offset_rot, convention="ros"),
+            spawn=sim_utils.PinholeCameraCfg().from_intrinsic_matrix(
+                intrinsic_matrix=intrinsics,
+                height=540,
+                width=960,
+                focal_length=38.0,
+                # clipping_range=(0.01, 20),
+            ),
+            height=540,
+            width=960,
+            data_types=["distance_to_camera"],
+        )
+
+        # set aperture offsets to 0, as currently not supported for usd/ tiled camera
+        camera_tiled_cfg.spawn.horizontal_aperture_offset = 0
+        camera_tiled_cfg.spawn.vertical_aperture_offset = 0
+        camera_usd_cfg.spawn.horizontal_aperture_offset = 0
+        camera_usd_cfg.spawn.vertical_aperture_offset = 0
+        # init cameras
+        camera_tiled = TiledCamera(camera_tiled_cfg)
+        camera_usd = Camera(camera_usd_cfg)
+
+        # play sim
+        self.sim.reset()
+        self.sim.play()
+
+        # perform steps
+        for _ in range(5):
+            self.sim.step()
+
+        # update camera
+        camera_usd.update(self.dt)
+        camera_tiled.update(self.dt)
+
+        # filter nan and inf from output
+        cam_tiled_output = camera_tiled.data.output["depth"].clone()
+        cam_usd_output = camera_usd.data.output["distance_to_camera"].clone()
+        cam_tiled_output[torch.isnan(cam_tiled_output)] = 0
+        cam_tiled_output[torch.isinf(cam_tiled_output)] = 0
+        cam_usd_output[torch.isnan(cam_usd_output)] = 0
+        cam_usd_output[torch.isinf(cam_usd_output)] = 0
+
+        # check that both have the same intrinsic matrices
+        torch.testing.assert_close(camera_tiled.data.intrinsic_matrices[0], camera_usd.data.intrinsic_matrices[0])
+
+        # check the apertures
+        torch.testing.assert_close(
+            camera_usd._sensor_prims[0].GetHorizontalApertureAttr().Get(),
+            camera_tiled._sensor_prims[0].GetHorizontalApertureAttr().Get(),
+        )
+        torch.testing.assert_close(
+            camera_usd._sensor_prims[0].GetVerticalApertureAttr().Get(),
+            camera_tiled._sensor_prims[0].GetVerticalApertureAttr().Get(),
+        )
+
+        # check image data
+        # FIXME: The tiled camera output is not exactly equal to the usd camera output. This should be fixed with the
+        #        update to the new tiled camera implementation. Test will fail, if the difference between the images
+        #        disappears. Check again @pascal-roth.
+
+        # Intended test
+        # torch.testing.assert_close(
+        #     cam_tiled_output[..., 0],
+        #     cam_usd_output,
+        #     atol=5e-5,
+        #     rtol=5e-6,
+        # )
+        # current failure case
+        self.assertTrue(torch.max(torch.abs(cam_tiled_output[..., 0] - cam_usd_output)).item() > 1)
+
+        del camera_tiled
+        del camera_usd
+
     """
     Helper functions.
     """

source/extensions/omni.isaac.lab/test/sim/test_spawn_sensors.py

@@ -99,7 +99,14 @@ class TestSpawningSensors(unittest.TestCase):
             custom_attr: The custom attributes for sensor.
         """
         # delete custom attributes in the config that are not USD parameters
-        non_usd_cfg_param_names = ["func", "copy_from_source", "lock_camera", "visible", "semantic_tags"]
+        non_usd_cfg_param_names = [
+            "func",
+            "copy_from_source",
+            "lock_camera",
+            "visible",
+            "semantic_tags",
+            "from_intrinsic_matrix",
+        ]
         # get prim
         prim = prim_utils.get_prim_at_path(prim_path)
         for attr_name, attr_value in cfg.__dict__.items():