Unverified Commit b01447d6 authored by Pascal Roth's avatar Pascal Roth Committed by GitHub

Adds `distance_to_camera` datatype in `TiledCamera` (#889)

# Description

Added the data type "distance_to_camera" in the `TiledCamera` class to
be consistent with all other cameras. This data type is equal to the
"depth" datatype.

## 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`
- [x] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [ ] 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
parent 8ae68c66
[package] [package]
# Note: Semantic Versioning is used: https://semver.org/ # Note: Semantic Versioning is used: https://semver.org/
version = "0.22.4" version = "0.22.6"
# Description # Description
title = "Isaac Lab framework for Robot Learning" title = "Isaac Lab framework for Robot Learning"
......
Changelog
---------
0.22.6 (2024-08-29)
~~~~~~~~~~~~~~~~~~~
Added
^^^^^
* Added alternative data type "distance_to_camera" in :class:`omni.isaac.lab.sensors.TiledCamera` class to be
consistent with all other cameras (equal to type "depth").
0.22.5 (2024-08-29)
~~~~~~~~~~~~~~~~~~~
Fixed Fixed
^^^^^ ^^^^^
...@@ -6,9 +22,6 @@ Fixed ...@@ -6,9 +22,6 @@ Fixed
* Added test to check :attr:`omni.isaac.lab.sensors.RayCasterCamera.set_intrinsic_matrices` * Added test to check :attr:`omni.isaac.lab.sensors.RayCasterCamera.set_intrinsic_matrices`
Changelog
---------
0.22.4 (2024-08-29) 0.22.4 (2024-08-29)
~~~~~~~~~~~~~~~~~~~ ~~~~~~~~~~~~~~~~~~~
......
...@@ -12,6 +12,7 @@ from collections.abc import Sequence ...@@ -12,6 +12,7 @@ from collections.abc import Sequence
from tensordict import TensorDict from tensordict import TensorDict
from typing import TYPE_CHECKING, Any from typing import TYPE_CHECKING, Any
import carb
import omni.usd import omni.usd
import warp as wp import warp as wp
from omni.isaac.core.prims import XFormPrimView from omni.isaac.core.prims import XFormPrimView
...@@ -37,7 +38,8 @@ class TiledCamera(Camera): ...@@ -37,7 +38,8 @@ class TiledCamera(Camera):
The following sensor types are supported: The following sensor types are supported:
- ``"rgb"``: A rendered color image. - ``"rgb"``: A rendered color image.
- ``"depth"``: An image containing the distance to camera optical center. - ``"distance_to_camera"``: An image containing the distance to camera optical center.
- ``"depth"``: An alias for ``"distance_to_camera"``.
.. attention:: .. attention::
Please note that the fidelity of RGB images may be lower than the standard camera sensor due to the Please note that the fidelity of RGB images may be lower than the standard camera sensor due to the
...@@ -54,8 +56,12 @@ class TiledCamera(Camera): ...@@ -54,8 +56,12 @@ class TiledCamera(Camera):
cfg: TiledCameraCfg cfg: TiledCameraCfg
"""The configuration parameters.""" """The configuration parameters."""
SUPPORTED_TYPES: set[str] = {"rgb", "depth"} SUPPORTED_TYPES: set[str] = {"rgb", "distance_to_camera", "depth"}
"""The set of sensor types that are supported.""" """The set of sensor types that are supported.
.. note::
The ``"depth"`` type is an alias for ``"distance_to_camera"``.
"""
def __init__(self, cfg: TiledCameraCfg): def __init__(self, cfg: TiledCameraCfg):
"""Initializes the tiled camera sensor. """Initializes the tiled camera sensor.
...@@ -157,12 +163,23 @@ class TiledCamera(Camera): ...@@ -157,12 +163,23 @@ class TiledCamera(Camera):
# start the orchestrator (if not already started) # start the orchestrator (if not already started)
rep.orchestrator._orchestrator._is_started = True rep.orchestrator._orchestrator._is_started = True
# check the data_types and remove "depth" if "distance_to_camera" is requested too
if "depth" in self.cfg.data_types and "distance_to_camera" in self.cfg.data_types:
carb.log_warn(
"Both 'depth' and 'distance_to_camera' are requested which are the same. 'depth' will be ignored."
)
self.cfg.data_types.remove("depth")
# NOTE: internally, "distance_to_camera" is named "depth", name is adjusted here
data_type = self.cfg.data_types.copy()
if "distance_to_camera" in data_type:
data_type.remove("distance_to_camera")
data_type.append("depth")
# Create a tiled sensor from the camera prims # Create a tiled sensor from the camera prims
rep_sensor = rep.create.tiled_sensor( rep_sensor = rep.create.tiled_sensor(
cameras=self._view.prim_paths, cameras=self._view.prim_paths,
camera_resolution=[self.image_shape[1], self.image_shape[0]], camera_resolution=[self.image_shape[1], self.image_shape[0]],
tiled_resolution=self._tiled_image_shape(), tiled_resolution=self._tiled_image_shape(),
output_types=self.cfg.data_types, output_types=data_type,
) )
# Get render product # Get render product
render_prod_path = rep.create.render_product(camera=rep_sensor, resolution=self._tiled_image_shape()) render_prod_path = rep.create.render_product(camera=rep_sensor, resolution=self._tiled_image_shape())
...@@ -198,7 +215,7 @@ class TiledCamera(Camera): ...@@ -198,7 +215,7 @@ class TiledCamera(Camera):
wp.from_torch(self._data.output[data_type]), # zero-copy alias wp.from_torch(self._data.output[data_type]), # zero-copy alias
*list(self._data.output[data_type].shape[1:]), # height, width, num_channels *list(self._data.output[data_type].shape[1:]), # height, width, num_channels
self._tiling_grid_shape()[0], # num_tiles_x self._tiling_grid_shape()[0], # num_tiles_x
offset if data_type == "depth" else 0, offset if data_type == "distance_to_camera" or data_type == "depth" else 0,
], ],
device=self.device, device=self.device,
) )
...@@ -232,8 +249,9 @@ class TiledCamera(Camera): ...@@ -232,8 +249,9 @@ class TiledCamera(Camera):
data_dict["rgb"] = torch.zeros( data_dict["rgb"] = torch.zeros(
(self._view.count, self.cfg.height, self.cfg.width, 3), device=self.device (self._view.count, self.cfg.height, self.cfg.width, 3), device=self.device
).contiguous() ).contiguous()
if "depth" in self.cfg.data_types: for data_type in ["distance_to_camera", "depth"]:
data_dict["depth"] = torch.zeros( if data_type in self.cfg.data_types:
data_dict[data_type] = torch.zeros(
(self._view.count, self.cfg.height, self.cfg.width, 1), device=self.device (self._view.count, self.cfg.height, self.cfg.width, 1), device=self.device
).contiguous() ).contiguous()
self._data.output = TensorDict(data_dict, batch_size=self._view.count, device=self.device) self._data.output = TensorDict(data_dict, batch_size=self._view.count, device=self.device)
......
...@@ -44,7 +44,7 @@ class TestTiledCamera(unittest.TestCase): ...@@ -44,7 +44,7 @@ class TestTiledCamera(unittest.TestCase):
offset=TiledCameraCfg.OffsetCfg(pos=(0.0, 0.0, 4.0), rot=(0.0, 0.0, 1.0, 0.0), convention="ros"), offset=TiledCameraCfg.OffsetCfg(pos=(0.0, 0.0, 4.0), rot=(0.0, 0.0, 1.0, 0.0), convention="ros"),
prim_path="/World/Camera", prim_path="/World/Camera",
update_period=0, update_period=0,
data_types=["rgb", "depth"], data_types=["rgb", "distance_to_camera"],
spawn=sim_utils.PinholeCameraCfg( spawn=sim_utils.PinholeCameraCfg(
focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1.0e5) focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1.0e5)
), ),
...@@ -217,6 +217,33 @@ class TestTiledCamera(unittest.TestCase): ...@@ -217,6 +217,33 @@ class TestTiledCamera(unittest.TestCase):
self.assertGreater(im_data[1].mean().item(), 0.0) self.assertGreater(im_data[1].mean().item(), 0.0)
del camera del camera
def test_data_types(self):
"""Test single camera initialization."""
# Create camera
camera_cfg_distance = copy.deepcopy(self.camera_cfg)
camera_cfg_distance.data_types = ["distance_to_camera"]
camera_cfg_distance.prim_path = "/World/CameraDistance"
camera_distance = TiledCamera(camera_cfg_distance)
camera_cfg_depth = copy.deepcopy(self.camera_cfg)
camera_cfg_depth.data_types = ["depth"]
camera_cfg_depth.prim_path = "/World/CameraDepth"
camera_depth = TiledCamera(camera_cfg_depth)
camera_cfg_both = copy.deepcopy(self.camera_cfg)
camera_cfg_both.data_types = ["distance_to_camera", "depth"]
camera_cfg_both.prim_path = "/World/CameraBoth"
camera_both = TiledCamera(camera_cfg_both)
# Play sim
self.sim.reset()
# Check if camera is initialized
self.assertTrue(camera_distance.is_initialized)
self.assertTrue(camera_depth.is_initialized)
self.assertTrue(camera_both.is_initialized)
self.assertListEqual(list(camera_distance.data.output.keys()), ["distance_to_camera"])
self.assertListEqual(list(camera_depth.data.output.keys()), ["depth"])
self.assertListEqual(list(camera_both.data.output.keys()), ["distance_to_camera"])
del camera_distance, camera_depth, camera_both
def test_depth_only_camera(self): def test_depth_only_camera(self):
"""Test initialization with only depth.""" """Test initialization with only depth."""
...@@ -225,7 +252,7 @@ class TestTiledCamera(unittest.TestCase): ...@@ -225,7 +252,7 @@ class TestTiledCamera(unittest.TestCase):
# Create camera # Create camera
camera_cfg = copy.deepcopy(self.camera_cfg) camera_cfg = copy.deepcopy(self.camera_cfg)
camera_cfg.data_types = ["depth"] camera_cfg.data_types = ["distance_to_camera"]
camera_cfg.prim_path = "/World/Origin_.*/CameraSensor" camera_cfg.prim_path = "/World/Origin_.*/CameraSensor"
camera = TiledCamera(camera_cfg) camera = TiledCamera(camera_cfg)
# Check simulation parameter is set correctly # Check simulation parameter is set correctly
...@@ -237,7 +264,7 @@ class TestTiledCamera(unittest.TestCase): ...@@ -237,7 +264,7 @@ class TestTiledCamera(unittest.TestCase):
# Check if camera prim is set correctly and that it is a camera prim # Check if camera prim is set correctly and that it is a camera prim
self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_01/CameraSensor") self.assertEqual(camera._sensor_prims[1].GetPath().pathString, "/World/Origin_01/CameraSensor")
self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera) self.assertIsInstance(camera._sensor_prims[0], UsdGeom.Camera)
self.assertListEqual(list(camera.data.output.keys()), ["depth"]) self.assertListEqual(list(camera.data.output.keys()), ["distance_to_camera"])
# Simulate for a few steps # Simulate for a few steps
# note: This is a workaround to ensure that the textures are loaded. # note: This is a workaround to ensure that the textures are loaded.
......
...@@ -80,7 +80,7 @@ class CartpoleDepthCameraEnvCfg(CartpoleRGBCameraEnvCfg): ...@@ -80,7 +80,7 @@ class CartpoleDepthCameraEnvCfg(CartpoleRGBCameraEnvCfg):
tiled_camera: TiledCameraCfg = TiledCameraCfg( tiled_camera: TiledCameraCfg = TiledCameraCfg(
prim_path="/World/envs/env_.*/Camera", prim_path="/World/envs/env_.*/Camera",
offset=TiledCameraCfg.OffsetCfg(pos=(-7.0, 0.0, 3.0), rot=(0.9945, 0.0, 0.1045, 0.0), convention="world"), offset=TiledCameraCfg.OffsetCfg(pos=(-7.0, 0.0, 3.0), rot=(0.9945, 0.0, 0.1045, 0.0), convention="world"),
data_types=["depth"], data_types=["distance_to_camera"],
spawn=sim_utils.PinholeCameraCfg( spawn=sim_utils.PinholeCameraCfg(
focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 20.0) focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 20.0)
), ),
...@@ -172,7 +172,7 @@ class CartpoleCameraEnv(DirectRLEnv): ...@@ -172,7 +172,7 @@ class CartpoleCameraEnv(DirectRLEnv):
self._cartpole.set_joint_effort_target(self.actions, joint_ids=self._cart_dof_idx) self._cartpole.set_joint_effort_target(self.actions, joint_ids=self._cart_dof_idx)
def _get_observations(self) -> dict: def _get_observations(self) -> dict:
data_type = "rgb" if "rgb" in self.cfg.tiled_camera.data_types else "depth" data_type = "rgb" if "rgb" in self.cfg.tiled_camera.data_types else "distance_to_camera"
observations = {"policy": self._tiled_camera.data.output[data_type].clone()} observations = {"policy": self._tiled_camera.data.output[data_type].clone()}
if self.cfg.write_image_to_file: if self.cfg.write_image_to_file:
......
...@@ -96,7 +96,7 @@ class SensorsSceneCfg(InteractiveSceneCfg): ...@@ -96,7 +96,7 @@ class SensorsSceneCfg(InteractiveSceneCfg):
update_period=0.1, update_period=0.1,
height=480, height=480,
width=640, width=640,
data_types=["rgb", "depth"], data_types=["rgb", "distance_to_camera"],
spawn=None, # the camera is already spawned in the scene spawn=None, # the camera is already spawned in the scene
offset=TiledCameraCfg.OffsetCfg(pos=(0.510, 0.0, 0.015), rot=(0.5, -0.5, 0.5, -0.5), convention="ros"), offset=TiledCameraCfg.OffsetCfg(pos=(0.510, 0.0, 0.015), rot=(0.5, -0.5, 0.5, -0.5), convention="ros"),
) )
...@@ -221,7 +221,7 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): ...@@ -221,7 +221,7 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
print("-------------------------------") print("-------------------------------")
print(scene["tiled_camera"]) print(scene["tiled_camera"])
print("Received shape of rgb image: ", scene["tiled_camera"].data.output["rgb"].shape) print("Received shape of rgb image: ", scene["tiled_camera"].data.output["rgb"].shape)
print("Received shape of depth image: ", scene["tiled_camera"].data.output["depth"].shape) print("Received shape of depth image: ", scene["tiled_camera"].data.output["distance_to_camera"].shape)
print("-------------------------------") print("-------------------------------")
print(scene["raycast_camera"]) print(scene["raycast_camera"])
print("Received shape of depth: ", scene["raycast_camera"].data.output["distance_to_image_plane"].shape) print("Received shape of depth: ", scene["raycast_camera"].data.output["distance_to_image_plane"].shape)
...@@ -242,7 +242,7 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): ...@@ -242,7 +242,7 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
# compare generated Depth images across different cameras # compare generated Depth images across different cameras
depth_images = [ depth_images = [
scene["camera"].data.output["distance_to_image_plane"][0], scene["camera"].data.output["distance_to_image_plane"][0],
scene["tiled_camera"].data.output["depth"][0, ..., 0], scene["tiled_camera"].data.output["distance_to_camera"][0, ..., 0],
scene["raycast_camera"].data.output["distance_to_image_plane"][0], scene["raycast_camera"].data.output["distance_to_image_plane"][0],
] ]
save_images_grid( save_images_grid(
...@@ -250,7 +250,7 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): ...@@ -250,7 +250,7 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
cmap="turbo", cmap="turbo",
subtitles=["Camera", "TiledCamera", "RaycasterCamera"], subtitles=["Camera", "TiledCamera", "RaycasterCamera"],
title="Depth Image: Cam0", title="Depth Image: Cam0",
filename=os.path.join(output_dir, "depth", f"{count:04d}.jpg"), filename=os.path.join(output_dir, "distance_to_camera", f"{count:04d}.jpg"),
) )
# save all tiled RGB images # save all tiled RGB images
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment