From b4723ba2eae0fdeda1d07c175234bca16b5f356d Mon Sep 17 00:00:00 2001 From: James Tigue <166445701+jtigue-bdai@users.noreply.github.com> Date: Thu, 19 Dec 2024 14:45:37 -0500 Subject: [PATCH 1/3] change how camera intriniscs are used for creation and update --- .../omni/isaac/lab/sensors/camera/camera.py | 67 +++++++------------ .../lab/sim/spawners/sensors/sensors_cfg.py | 29 ++++---- .../omni/isaac/lab/utils/sensors.py | 60 +++++++++++++++++ .../test/sensors/test_camera.py | 29 ++++---- .../test/sensors/test_tiled_camera.py | 4 -- 5 files changed, 114 insertions(+), 75 deletions(-) create mode 100644 source/extensions/omni.isaac.lab/omni/isaac/lab/utils/sensors.py diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py index 9fa9cf1862..3cbbca8599 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py @@ -19,6 +19,7 @@ from pxr import UsdGeom import omni.isaac.lab.sim as sim_utils +import omni.isaac.lab.utils.sensors as sensor_utils from omni.isaac.lab.utils import to_camel_case from omni.isaac.lab.utils.array import convert_to_torch from omni.isaac.lab.utils.math import ( @@ -203,28 +204,28 @@ def image_shape(self) -> tuple[int, int]: """ def set_intrinsic_matrices( - self, matrices: torch.Tensor, focal_length: float = 1.0, env_ids: Sequence[int] | None = None + self, matrices: torch.Tensor, focal_length: float | None = None, env_ids: Sequence[int] | None = None ): """Set parameters of the USD camera from its intrinsic matrix. - The intrinsic matrix and focal length are used to set the following parameters to the USD camera: + The intrinsic matrix is used to set the following parameters to the USD camera: - - ``focal_length``: The focal length of the camera. - - ``horizontal_aperture``: The horizontal aperture of the camera. - - ``vertical_aperture``: The vertical aperture of the camera. - - ``horizontal_aperture_offset``: The horizontal offset of the camera. - - ``vertical_aperture_offset``: The vertical offset of the camera. + - ``focal_length``: The focal length of the camera. + - ``horizontal_aperture``: The horizontal aperture of the camera. + - ``vertical_aperture``: The vertical aperture of the camera. + - ``horizontal_aperture_offset``: The horizontal offset of the camera. + - ``vertical_aperture_offset``: The vertical offset of the camera. - .. warning:: + .. warning:: - Due to limitations of Omniverse camera, we need to assume that the camera is a spherical lens, - i.e. has square pixels, and the optical center is centered at the camera eye. If this assumption - is not true in the input intrinsic matrix, then the camera will not set up correctly. + Due to limitations of Omniverse camera, we need to assume that the camera is a spherical lens, + i.e. has square pixels, and the optical center is centered at the camera eye. If this assumption + is not true in the input intrinsic matrix, then the camera will not set up correctly. - Args: - matrices: The intrinsic matrices for the camera. Shape is (N, 3, 3). - focal_length: Focal length to use when computing aperture values (in cm). Defaults to 1.0. - env_ids: A sensor ids to manipulate. Defaults to None, which means all sensor indices. + Args: + matrices: The intrinsic matrices for the camera. Shape is (N, 3, 3). + focal_length: Perspective focal length (in cm) used to calculate pixel size. Defaults to None. If None, focal_length will be calculated 1 / width. + env_ids: A sensor ids to manipulate. Defaults to None, which means all sensor indices. """ # resolve env_ids if env_ids is None: @@ -236,27 +237,12 @@ def set_intrinsic_matrices( matrices = np.asarray(matrices, dtype=float) # iterate over env_ids for i, intrinsic_matrix in zip(env_ids, matrices): - # extract parameters from matrix - f_x = intrinsic_matrix[0, 0] - c_x = intrinsic_matrix[0, 2] - f_y = intrinsic_matrix[1, 1] - c_y = intrinsic_matrix[1, 2] - # get viewport parameters + height, width = self.image_shape - height, width = float(height), float(width) - # resolve parameters for usd camera - params = { - "focal_length": focal_length, - "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, - } - - # 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: - omni.log.warn("Camera aperture offsets are not supported by Omniverse. These parameters are ignored.") + + params = sensor_utils.convert_camera_intrinsics_to_usd( + intrinsic_matrix=intrinsic_matrix.reshape(-1), height=height, width=width, focal_length=focal_length + ) # change data for corresponding camera index sensor_prim = self._sensor_prims[i] @@ -581,18 +567,17 @@ def _update_intrinsic_matrices(self, env_ids: Sequence[int]): # Get corresponding sensor prim sensor_prim = self._sensor_prims[i] # get camera parameters + # currently rendering does not use aperture offsets or vertical aperture 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 # 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 + f_y = f_x + c_x = width * 0.5 + c_y = height * 0.5 # create intrinsic matrix for depth linear self._data.intrinsic_matrices[i, 0, 0] = f_x self._data.intrinsic_matrices[i, 0, 2] = c_x diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/sim/spawners/sensors/sensors_cfg.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/sim/spawners/sensors/sensors_cfg.py index 7e97c0d93e..15399ab33a 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/sim/spawners/sensors/sensors_cfg.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/sim/spawners/sensors/sensors_cfg.py @@ -8,6 +8,7 @@ from collections.abc import Callable from typing import Literal +import omni.isaac.lab.utils.sensors as sensor_utils from omni.isaac.lab.sim.spawners.spawner_cfg import SpawnerCfg from omni.isaac.lab.utils import configclass @@ -102,7 +103,7 @@ def from_intrinsic_matrix( width: int, height: int, clipping_range: tuple[float, float] = (0.01, 1e6), - focal_length: float = 24.0, + focal_length: float | None = None, focus_distance: float = 400.0, f_stop: float = 0.0, projection_type: str = "pinhole", @@ -129,7 +130,8 @@ def from_intrinsic_matrix( 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. + focal_length: Perspective focal length (in cm) used to calculate pixel size. Defaults to None. If None + focal_length will be calculated 1 / width. 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". @@ -142,27 +144,20 @@ def from_intrinsic_matrix( 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 + usd_camera_params = sensor_utils.convert_camera_intrinsics_to_usd( + intrinsic_matrix=intrinsic_matrix, height=height, width=width, focal_length=focal_length + ) return cls( projection_type=projection_type, clipping_range=clipping_range, - focal_length=focal_length, + focal_length=usd_camera_params["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, + horizontal_aperture=usd_camera_params["horizontal_aperture"], + vertical_aperture=usd_camera_params["vertical_aperture"], + horizontal_aperture_offset=usd_camera_params["horizontal_aperture_offset"], + vertical_aperture_offset=usd_camera_params["vertical_aperture_offset"], lock_camera=lock_camera, ) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/utils/sensors.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/utils/sensors.py new file mode 100644 index 0000000000..1fafb783e4 --- /dev/null +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/utils/sensors.py @@ -0,0 +1,60 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import omni + + +def convert_camera_intrinsics_to_usd( + intrinsic_matrix: list[float], width: int, height: int, focal_length: float | None = None +) -> dict: + """Creates USD camera properties from camera intrinsics and resolution. + + 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: Perspective focal length (in cm) used to calculate pixel size. Defaults to None. If None + focal_length will be calculated 1 / width. + + Returns: + A dictionary of USD camera parameters for focal_length, horizontal_aperture, vertical_aperture, + horizontal_aperture_offset, and vertical_aperture_offset. + """ + usd_params = dict + + # extract parameters from matrix + f_x = intrinsic_matrix[0] + f_y = intrinsic_matrix[4] + c_x = intrinsic_matrix[2] + c_y = intrinsic_matrix[5] + + # warn about non-square pixels + if abs(f_x - f_y) > 1e-4: + omni.log.warn("Camera non square pixels are not supported by Omniverse. The average of f_x and f_y are used.") + + # warn about aperture offsets + if abs((c_x - float(width) / 2) > 1e-4 or (c_y - float(height) / 2) > 1e-4): + omni.log.warn( + "Camera aperture offsets are not supported by Omniverse. c_x and c_y will be half of width and height" + ) + + # calculate usd camera parameters + # pixel_size does not need to be exact it is just used for consistent sizing of aperture and focal_length + # https://docs.omniverse.nvidia.com/isaacsim/latest/features/sensors_simulation/isaac_sim_sensors_camera.html#calibrated-camera-sensors + if focal_length is None: + pixel_size = 1 / float(width) + else: + pixel_size = focal_length / ((f_x + f_y) / 2) + + usd_params = { + "horizontal_aperture": pixel_size * float(width), + "vertical_aperture": pixel_size * float(height), + "focal_length": pixel_size * (f_x + f_y) / 2, # The focal length in mm + "horizontal_aperture_offset": 0.0, + "vertical_aperture_offset": 0.0, + } + + return usd_params diff --git a/source/extensions/omni.isaac.lab/test/sensors/test_camera.py b/source/extensions/omni.isaac.lab/test/sensors/test_camera.py index d4c520eaa6..896b726da9 100644 --- a/source/extensions/omni.isaac.lab/test/sensors/test_camera.py +++ b/source/extensions/omni.isaac.lab/test/sensors/test_camera.py @@ -42,6 +42,10 @@ QUAT_OPENGL = [0.33985113, 0.17591988, 0.42470818, 0.82047324] QUAT_WORLD = [-0.3647052, -0.27984815, -0.1159169, 0.88047623] +# resolutions +HEIGHT = 240 +WIDTH = 320 + class TestCamera(unittest.TestCase): """Test for USD Camera sensor.""" @@ -49,8 +53,8 @@ class TestCamera(unittest.TestCase): def setUp(self): """Create a blank new stage for each test.""" self.camera_cfg = CameraCfg( - height=128, - width=128, + height=HEIGHT, + width=WIDTH, prim_path="/World/Camera", update_period=0, data_types=["distance_to_image_plane"], @@ -271,23 +275,23 @@ def test_camera_init_intrinsic_matrix(self): # 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, + height=HEIGHT, + width=WIDTH, 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, + width=WIDTH, + height=HEIGHT, clipping_range=(0.1, 1.0e5), ), ) @@ -369,7 +373,7 @@ def test_intrinsic_matrix(self): # play sim self.sim.reset() # Desired properties (obtained from realsense camera at 320x240 resolution) - rs_intrinsic_matrix = [229.31640625, 0.0, 164.810546875, 0.0, 229.826171875, 122.1650390625, 0.0, 0.0, 1.0] + rs_intrinsic_matrix = [229.8, 0.0, 160.0, 0.0, 229.8, 120.0, 0.0, 0.0, 1.0] rs_intrinsic_matrix = torch.tensor(rs_intrinsic_matrix, device=camera.device).reshape(3, 3).unsqueeze(0) # Set matrix into simulator camera.set_intrinsic_matrices(rs_intrinsic_matrix.clone()) @@ -387,11 +391,10 @@ def test_intrinsic_matrix(self): # 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[0, 1, 1], camera.data.intrinsic_matrices[0, 1, 1]) + torch.testing.assert_close(rs_intrinsic_matrix[0, 0, 2], camera.data.intrinsic_matrices[0, 0, 2]) + torch.testing.assert_close(rs_intrinsic_matrix[0, 1, 2], camera.data.intrinsic_matrices[0, 1, 2]) def test_depth_clipping(self): """Test depth clipping. diff --git a/source/extensions/omni.isaac.lab/test/sensors/test_tiled_camera.py b/source/extensions/omni.isaac.lab/test/sensors/test_tiled_camera.py index 66dfd5b0af..1d8b764d22 100644 --- a/source/extensions/omni.isaac.lab/test/sensors/test_tiled_camera.py +++ b/source/extensions/omni.isaac.lab/test/sensors/test_tiled_camera.py @@ -1340,8 +1340,6 @@ def test_output_equal_to_usd_camera_intrinsics(self): intrinsic_matrix=intrinsics, height=540, width=960, - focal_length=38.0, - # clipping_range=(0.01, 20), ), height=540, width=960, @@ -1354,8 +1352,6 @@ def test_output_equal_to_usd_camera_intrinsics(self): intrinsic_matrix=intrinsics, height=540, width=960, - focal_length=38.0, - # clipping_range=(0.01, 20), ), height=540, width=960, From 6c420d065b40577c63dd56edeafaea4159f7f703 Mon Sep 17 00:00:00 2001 From: James Tigue Date: Fri, 3 Jan 2025 10:24:43 -0500 Subject: [PATCH 2/3] clean up docstring --- .../omni/isaac/lab/sensors/camera/camera.py | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py index 3cbbca8599..1808019af1 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/sensors/camera/camera.py @@ -208,24 +208,25 @@ def set_intrinsic_matrices( ): """Set parameters of the USD camera from its intrinsic matrix. - The intrinsic matrix is used to set the following parameters to the USD camera: + The intrinsic matrix is used to set the following parameters to the USD camera: - - ``focal_length``: The focal length of the camera. - - ``horizontal_aperture``: The horizontal aperture of the camera. - - ``vertical_aperture``: The vertical aperture of the camera. - - ``horizontal_aperture_offset``: The horizontal offset of the camera. - - ``vertical_aperture_offset``: The vertical offset of the camera. + - ``focal_length``: The focal length of the camera. + - ``horizontal_aperture``: The horizontal aperture of the camera. + - ``vertical_aperture``: The vertical aperture of the camera. + - ``horizontal_aperture_offset``: The horizontal offset of the camera. + - ``vertical_aperture_offset``: The vertical offset of the camera. - .. warning:: + .. warning:: - Due to limitations of Omniverse camera, we need to assume that the camera is a spherical lens, - i.e. has square pixels, and the optical center is centered at the camera eye. If this assumption - is not true in the input intrinsic matrix, then the camera will not set up correctly. + Due to limitations of Omniverse camera, we need to assume that the camera is a spherical lens, + i.e. has square pixels, and the optical center is centered at the camera eye. If this assumption + is not true in the input intrinsic matrix, then the camera will not set up correctly. - Args: - matrices: The intrinsic matrices for the camera. Shape is (N, 3, 3). - focal_length: Perspective focal length (in cm) used to calculate pixel size. Defaults to None. If None, focal_length will be calculated 1 / width. - env_ids: A sensor ids to manipulate. Defaults to None, which means all sensor indices. + Args: + matrices: The intrinsic matrices for the camera. Shape is (N, 3, 3). + focal_length: Perspective focal length (in cm) used to calculate pixel size. Defaults to None. If None, + focal_length will be calculated 1 / width. + env_ids: A sensor ids to manipulate. Defaults to None, which means all sensor indices. """ # resolve env_ids if env_ids is None: From 8b541b33498d2664621e1a4a339e8478ca5e8e9e Mon Sep 17 00:00:00 2001 From: James Tigue Date: Fri, 3 Jan 2025 10:36:36 -0500 Subject: [PATCH 3/3] update changelog --- .../omni.isaac.lab/config/extension.toml | 2 +- .../extensions/omni.isaac.lab/docs/CHANGELOG.rst | 14 ++++++++++++++ 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/source/extensions/omni.isaac.lab/config/extension.toml b/source/extensions/omni.isaac.lab/config/extension.toml index 6775254252..f654f4e0c7 100644 --- a/source/extensions/omni.isaac.lab/config/extension.toml +++ b/source/extensions/omni.isaac.lab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.30.3" +version = "0.30.4" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst index 2f8ce741d9..7a5ac5e938 100644 --- a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst @@ -1,6 +1,20 @@ Changelog --------- +0.30.4 (2025-01-03) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed :meth:`omni.isaac.lab.sensors.camera.camera.Camera.set_intrinsic_matrices` preventing setting of unused USD + camera parameters. +* Fixed :meth:`omni.isaac.lab.sensors.camera.camera.Camera._update_intrinsic_matrices` preventing unused USD camera + parameters from being used to calculate :attr:`omni.isaac.lab.sensors.camera.CameraData.intrinsic_matrices` +* Fixed :meth:`omni.isaac.lab.spawners.sensors.sensors_cfg.PinholeCameraCfg.from_intrinsic_matrix` preventing setting of + unused USD camera parameters. + + 0.30.3 (2025-01-02) ~~~~~~~~~~~~~~~~~~~