Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adds distance_to_camera datatype in TiledCamera #889

Merged
merged 8 commits into from
Sep 5, 2024
Merged
Show file tree
Hide file tree
Changes from 4 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions source/extensions/omni.isaac.lab/docs/CHANGELOG.rst
Original file line number Diff line number Diff line change
@@ -1,3 +1,10 @@
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").


Changelog
---------

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@
from tensordict import TensorDict
from typing import TYPE_CHECKING, Any

import carb
import omni.usd
import warp as wp
from omni.isaac.core.prims import XFormPrimView
Expand All @@ -37,7 +38,8 @@ class TiledCamera(Camera):
The following sensor types are supported:

- ``"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::
Please note that the fidelity of RGB images may be lower than the standard camera sensor due to the
Expand All @@ -54,7 +56,7 @@ class TiledCamera(Camera):
cfg: TiledCameraCfg
"""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."""
pascal-roth marked this conversation as resolved.
Show resolved Hide resolved

def __init__(self, cfg: TiledCameraCfg):
Expand Down Expand Up @@ -157,12 +159,23 @@ def _initialize_impl(self):

# start the orchestrator (if not already started)
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
rep_sensor = rep.create.tiled_sensor(
cameras=self._view.prim_paths,
camera_resolution=[self.image_shape[1], self.image_shape[0]],
tiled_resolution=self._tiled_image_shape(),
output_types=self.cfg.data_types,
output_types=data_type,
)
# Get render product
render_prod_path = rep.create.render_product(camera=rep_sensor, resolution=self._tiled_image_shape())
Expand Down Expand Up @@ -198,7 +211,7 @@ def _update_buffers_impl(self, env_ids: Sequence[int]):
wp.from_torch(self._data.output[data_type]), # zero-copy alias
*list(self._data.output[data_type].shape[1:]), # height, width, num_channels
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,
)
Expand Down Expand Up @@ -232,10 +245,11 @@ def _create_buffers(self):
data_dict["rgb"] = torch.zeros(
(self._view.count, self.cfg.height, self.cfg.width, 3), device=self.device
).contiguous()
if "depth" in self.cfg.data_types:
data_dict["depth"] = torch.zeros(
(self._view.count, self.cfg.height, self.cfg.width, 1), device=self.device
).contiguous()
for data_type in ["distance_to_camera", "depth"]:
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
).contiguous()
self._data.output = TensorDict(data_dict, batch_size=self._view.count, device=self.device)

def _tiled_image_shape(self) -> tuple[int, int]:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ def setUp(self):
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",
update_period=0,
data_types=["rgb", "depth"],
data_types=["rgb", "distance_to_camera"],
spawn=sim_utils.PinholeCameraCfg(
focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 1.0e5)
),
Expand Down Expand Up @@ -216,6 +216,33 @@ def test_rgb_only_camera(self):
self.assertGreater(im_data[1].mean().item(), 0.0)
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):
"""Test initialization with only depth."""

Expand All @@ -224,7 +251,7 @@ def test_depth_only_camera(self):

# Create camera
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 = TiledCamera(camera_cfg)
# Check simulation parameter is set correctly
Expand All @@ -236,7 +263,7 @@ def test_depth_only_camera(self):
# 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.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
# note: This is a workaround to ensure that the textures are loaded.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,7 @@ class CartpoleDepthCameraEnvCfg(CartpoleRGBCameraEnvCfg):
tiled_camera: TiledCameraCfg = TiledCameraCfg(
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"),
data_types=["depth"],
data_types=["distance_to_camera"],
spawn=sim_utils.PinholeCameraCfg(
focal_length=24.0, focus_distance=400.0, horizontal_aperture=20.955, clipping_range=(0.1, 20.0)
),
Expand Down Expand Up @@ -172,7 +172,7 @@ def _apply_action(self) -> None:
self._cartpole.set_joint_effort_target(self.actions, joint_ids=self._cart_dof_idx)

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()}

if self.cfg.write_image_to_file:
Expand Down
8 changes: 4 additions & 4 deletions source/standalone/demos/cameras.py
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,7 @@ class SensorsSceneCfg(InteractiveSceneCfg):
update_period=0.1,
height=480,
width=640,
data_types=["rgb", "depth"],
data_types=["rgb", "distance_to_camera"],
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"),
)
Expand Down Expand Up @@ -221,7 +221,7 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
print("-------------------------------")
print(scene["tiled_camera"])
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(scene["raycast_camera"])
print("Received shape of depth: ", scene["raycast_camera"].data.output["distance_to_image_plane"].shape)
Expand All @@ -242,15 +242,15 @@ def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene):
# compare generated Depth images across different cameras
depth_images = [
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],
]
save_images_grid(
depth_images,
cmap="turbo",
subtitles=["Camera", "TiledCamera", "RaycasterCamera"],
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
Expand Down
Loading