isaac-sim · Mayankm96 · Aug 29, 2024 · Jul 2, 2024 · Jul 2, 2024 · Jul 2, 2024
@@ -1,3 +1,11 @@
+Added
+^^^^^
+
+* Added possibility to initialize camera configurations with an intrinsic matrix by adding a new classmethod to the
+  :class:`omni.isaac.lab.sim.spawner.sensors.PinholeCameraCfg` class or respectively
+  :class:`omni.isaac.lab.sensors.ray_caster.patterns_cfg.PinholeCameraPatternCfg`.
+
+
 Changelog
 ---------
 

@@ -5,7 +5,6 @@
 
 from __future__ import annotations
 
-import math
 import numpy as np
 import re
 import torch
@@ -541,17 +540,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]):

@@ -96,6 +96,53 @@ class PinholeCameraPatternCfg(PatternBaseCfg):
     height: int = MISSING
     """Height of the image (in pixels)."""
 
+    @classmethod
+    def from_intrinsic_matrix(
+        cls,
+        focal_length: float,
+        intrinsic_matrix: list[float],
+        width: int,
+        height: int,
+    ) -> PinholeCameraPatternCfg:
+        """Create a PinholeCameraPatternCfg 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::
+            \\begin{bmatrix}
+            f_x & 0 & c_x \\\\
+            0 & f_y & c_y \\\\
+            0 & 0 & 1
+            \\end{bmatrix}
+
+        Args:
+            focal_length: Focal length of the camera (in cm).
+            intrinsic_matrix: Intrinsic matrix of the camera (shape: (9,)).
+            width: Width of the image (in pixels).
+            height: Height of the image (in pixels).
+
+        Returns:
+            PinholeCameraPatternCfg: The configuration for the pinhole camera pattern.
+        """
+        # 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,
+            horizontal_aperture_offset=horizontal_aperture_offset,
+            vertical_aperture_offset=vertical_aperture_offset,
+            width=width,
+            height=height,
+        )
+
 
 @configclass
 class BpearlPatternCfg(PatternBaseCfg):

@@ -357,6 +357,7 @@ def _compute_intrinsic_matrices(self):
         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
 

@@ -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",
+        "from_intrinsic_matrix",
+    ]
 
     # get camera prim
     prim = prim_utils.get_prim_at_path(prim_path)

@@ -3,6 +3,8 @@
 #
 # SPDX-License-Identifier: BSD-3-Clause
 
+from __future__ import annotations
+
 from collections.abc import Callable
 from typing import Literal
 
@@ -71,6 +73,72 @@ 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,
+        projection_type: str = "pinhole",
+        clipping_range: tuple[float, float] = (0.01, 1e6),
+        focal_length: float = 24.0,
+        focus_distance: float = 400.0,
+        f_stop: float = 0.0,
+        horizontal_aperture: float = 20.955,
+        horizontal_aperture_offset: float = 0.0,
+        vertical_aperture_offset: float = 0.0,
+        lock_camera: bool = True,
+    ) -> PinholeCameraCfg:
+        """Create a PinholeCameraCfg 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::
+            \\begin{bmatrix}
+            f_x & 0 & c_x \\\\
+            0 & f_y & c_y \\\\
+            0 & 0 & 1
+            \\end{bmatrix}
+
+        Args:
+            intrinsic_matrix: Intrinsic matrix of the camera (shape: (9,)).
+            width: Width of the image (in pixels).
+            height: Height of the image (in pixels).
+            projection_type: Type of projection to use for the camera. Defaults to "pinhole".
+            clipping_range: Near and far clipping distances (in m). Defaults to (0.01, 1e6).
+            focal_length: Perspective focal length (in cm). Defaults to 24.0cm.
+            focus_distance: Distance from the camera to the focus plane (in m). Defaults to 400.0.
+            f_stop: Lens aperture. Defaults to 0.0, which turns off focusing.
+            horizontal_aperture: Horizontal aperture (in mm). Defaults to 20.955mm.
+            horizontal_aperture_offset: Offsets Resolution horizontally. Defaults to 0.0.
+            vertical_aperture_offset: Offsets Resolution vertically. Defaults to 0.0.
+            lock_camera: Locks the camera in the Omniverse viewport. Defaults to True.
+
+        Returns:
+            PinholeCameraCfg: The configuration for the pinhole camera pattern.
+        """
+        # 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,
+            horizontal_aperture_offset=horizontal_aperture_offset,
+            vertical_aperture_offset=vertical_aperture_offset,
+            lock_camera=lock_camera,
+        )
+
 
 @configclass
 class FisheyeCameraCfg(PinholeCameraCfg):

@@ -231,6 +231,57 @@ def test_multi_camera_init(self):
                 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):
+        # 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()
+        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"],
+            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)

@@ -203,6 +203,58 @@ def test_camera_init_offset(self):
         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):
+        # 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(
+                focal_length=self.camera_cfg.pattern_cfg.focal_length,
+                intrinsic_matrix=intrinsic_matrix,
+                height=self.camera_cfg.pattern_cfg.height,
+                width=self.camera_cfg.pattern_cfg.width,
+            ),
+            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"],
+            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_multi_camera_init(self):
         """Test multi-camera initialization."""
         # create two cameras with different prim paths