Adds function to define camera configs through intrinsic matrix

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

@@ -1,3 +1,11 @@
+Added
+^^^^^
+
+* Added possibility to initialize cameras directly with the intrinsic matrix, which can be added as attribute to the
+  :class:`omni.isaac.lab.sim.spawner.sensors.PinholeCameraCfg` class or respectively
+  :class:`omni.isaac.lab.sensors.ray_caster.patterns_cfg.PinholeCameraPatternCfg`.
+
+
 Changelog
 ---------
 

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,18 @@ def __init__(self, cfg: CameraCfg):
             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()
+            # if intrinsic matrix is set, override aperture parameters in config
+            if self.cfg.spawn.intrinsic_matrix is not None:
+                # extract parameters from matrix
+                f_x = self.cfg.spawn.intrinsic_matrix[0]
+                c_x = self.cfg.spawn.intrinsic_matrix[2]
+                f_y = self.cfg.spawn.intrinsic_matrix[4]
+                c_y = self.cfg.spawn.intrinsic_matrix[5]
+                # resolve parameters for usd camera
+                self.cfg.spawn.horizontal_aperture = self.cfg.width * self.cfg.spawn.focal_length / f_x
+                self.cfg.spawn.vertical_aperture = self.cfg.height * self.cfg.spawn.focal_length / f_y
+                self.cfg.spawn.horizontal_aperture_offset = (c_x - self.cfg.width / 2) / f_x
+                self.cfg.spawn.vertical_aperture_offset = (c_y - self.cfg.height / 2) / f_y
             # spawn the asset
             self.cfg.spawn.func(
                 self.cfg.prim_path, self.cfg.spawn, translation=self.cfg.offset.pos, orientation=rot_offset
@@ -541,17 +552,21 @@ def _update_intrinsic_matrices(self, env_ids: Sequence[int]):
             # 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_cfg.py

@@ -91,6 +91,22 @@ class PinholeCameraPatternCfg(PatternBaseCfg):
     """Offsets Resolution/Film gate horizontally. Defaults to 0.0."""
     vertical_aperture_offset: float = 0.0
     """Offsets Resolution/Film gate vertically. Defaults to 0.0."""
+    intrinsic_matrix: tuple[float] | None = None
+    """Intrinsic matrix of the camera. Defaults to None.
+
+    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::
+        \\begin{bmatrix}
+        f_x & 0 & c_x \\\\
+        0 & f_y & c_y \\\\
+        0 & 0 & 1
+        \\end{bmatrix}
+
+    Note:
+        In the case both, horizontal aperture and intrinsic matrix is defined, the intrinsic matrix will be used.
+    """
     width: int = MISSING
     """Width of the image (in pixels)."""
     height: int = MISSING

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

@@ -346,17 +346,23 @@ def _compute_intrinsic_matrices(self):
         """Computes the intrinsic matrices for the camera based on the config provided."""
         # get the sensor properties
         pattern_cfg = self.cfg.pattern_cfg
-        # 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
-        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
-        self._data.intrinsic_matrices[:, 0, 0] = f_x
-        self._data.intrinsic_matrices[:, 0, 2] = c_x
-        self._data.intrinsic_matrices[:, 1, 1] = f_y
-        self._data.intrinsic_matrices[:, 1, 2] = c_y
+        if hasattr(pattern_cfg, "intrinsic_matrix") and pattern_cfg.intrinsic_matrix is not None:
+            # use the provided intrinsic matrix
+            matrix = torch.tensor(pattern_cfg.intrinsic_matrix, device=self._device).reshape(3, 3)
+            self._data.intrinsic_matrices = matrix.unsqueeze(0).repeat(self._view.count, 1, 1)
+        else:
+            # 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
+            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
+            self._data.intrinsic_matrices[:, 0, 0] = f_x
+            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

@@ -100,7 +100,14 @@ def spawn_camera(
     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"]
+    non_usd_cfg_param_names = [
+        "func",
+        "copy_from_source",
+        "lock_camera",
+        "visible",
+        "semantic_tags",
+        "intrinsic_matrix",
+    ]
 
     # get camera prim
     prim = prim_utils.get_prim_at_path(prim_path)

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

@@ -64,6 +64,22 @@ class PinholeCameraCfg(SpawnerCfg):
     """Offsets Resolution/Film gate horizontally. Defaults to 0.0."""
     vertical_aperture_offset: float = 0.0
     """Offsets Resolution/Film gate vertically. Defaults to 0.0."""
+    intrinsic_matrix: tuple[float] | None = None
+    """Intrinsic matrix of the camera. Defaults to None.
+
+    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::
+        \\begin{bmatrix}
+        f_x & 0 & c_x \\\\
+        0 & f_y & c_y \\\\
+        0 & 0 & 1
+        \\end{bmatrix}
+
+    Note:
+        In the case both, aperture parameters and intrinsic matrix is defined, the intrinsic matrix will be used.
+    """
     lock_camera: bool = True
     """Locks the camera in the Omniverse viewport. Defaults to True.
 

source/extensions/omni.isaac.lab_assets/omni/isaac/lab_assets/stereolabs.py

@@ -0,0 +1,63 @@
+# Copyright (c) 2022-2024, The Isaac Lab Project Developers.
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+"""Configuration for Sterolabs Depth Cameras."""
+
+
+from omni.isaac.lab.sensors import CameraCfg, RayCasterCameraCfg, patterns
+from omni.isaac.lab.sim.spawners import PinholeCameraCfg
+
+##
+# Configuration as RayCasterCameraCfg
+##
+
+ZED_X_NARROW = RayCasterCameraCfg(
+    pattern_cfg=patterns.PinholeCameraPatternCfg(
+        focal_length=4.0,
+        intrinsic_matrix=[380.0831, 0.0, 467.7916, 0.0, 380.0831, 262.0532, 0.0, 0.0, 1.0],
+        height=540,
+        width=960,
+    ),
+    debug_vis=True,
+    max_distance=20,
+    data_types=["distance_to_image_plane"],
+)
+
+ZED_X_MINI_WIDE = RayCasterCameraCfg(
+    pattern_cfg=patterns.PinholeCameraPatternCfg(
+        focal_length=2.2,
+        intrinsic_matrix=[369.7771, 0.0, 489.9926, 0.0, 369.7771, 275.9385, 0.0, 0.0, 1.0],
+        height=540,
+        width=960,
+    ),
+    debug_vis=True,
+    max_distance=20,
+    data_types=["distance_to_image_plane"],
+)
+
+
+##
+# Configuration as USD Camera
+##
+
+ZED_X_NARROW_USD = CameraCfg(
+    spawn=PinholeCameraCfg(
+        focal_length=4.0,
+        intrinsic_matrix=[380.0831, 0.0, 467.7916, 0.0, 380.0831, 262.0532, 0.0, 0.0, 1.0],
+    ),
+    height=540,
+    width=960,
+    data_types=["distance_to_image_plane", "rgb"],
+)
+
+ZED_X_MINI_WIDE_USD = CameraCfg(
+    spawn=PinholeCameraCfg(
+        focal_length=2.2,
+        intrinsic_matrix=[369.7771, 0.0, 489.9926, 0.0, 369.7771, 275.9385, 0.0, 0.0, 1.0],
+    ),
+    height=540,
+    width=960,
+    data_types=["distance_to_image_plane", "rgb"],
+)