Unverified Commit 4ce87f84 authored by Mayank Mittal's avatar Mayank Mittal Committed by GitHub

Fixes camera MDP term name and reprojection docstrings (#1130)

# Description

For all observation-related terms, we follow the noun name for the
function. However, the camera one was called `grab_images`. This MR
renames it to `image` to make it consistent with the others.

Also the docstring for the math-utils were messed up. According to
Google's docstyle, the first line should be a short summary and a
detailed summary should move to the next paragraph. This MR fixes that
as well.

Lastly, the name of the function
`convert_perspective_depth_to_orthoginal_depth` has too much redundancy.
The MR renames it to `orthogonalize_perpsective_depth`.

All these are breaking changes with no backwards compatibility (since
they were added post-last release anyway).

## Type of change

- Breaking change (fix or feature that would cause existing
functionality to not work as expected)
- This change requires a documentation update

## 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
- [ ] 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

---------
Signed-off-by: 's avatarMayank Mittal <12863862+Mayankm96@users.noreply.github.com>
parent 087535b9
[package] [package]
# Note: Semantic Versioning is used: https://semver.org/ # Note: Semantic Versioning is used: https://semver.org/
version = "0.24.13" version = "0.24.16"
# Description # Description
title = "Isaac Lab framework for Robot Learning" title = "Isaac Lab framework for Robot Learning"
......
Changelog Changelog
--------- ---------
0.22.15 (2024-09-20) 0.24.16 (2024-10-03)
~~~~~~~~~~~~~~~~~~~~
Changed
^^^^^^^
* Renamed the observation function :meth:`grab_images` to :meth:`image` to follow convention of noun-based naming.
* Renamed the function :meth:`convert_perspective_depth_to_orthogonal_depth` to a shorter name
:meth:`omni.isaac.lab.utils.math.orthogonalize_perspective_depth`.
0.24.15 (2024-09-20)
~~~~~~~~~~~~~~~~~~~~ ~~~~~~~~~~~~~~~~~~~~
Added Added
^^^^^ ^^^^^
* Added :meth:`grab_images` to be able to use images for an observation term in manager based environments * Added :meth:`grab_images` to be able to use images for an observation term in manager-based environments.
0.24.14 (2024-09-20) 0.24.14 (2024-09-20)
~~~~~~~~~~~~~~~~~~~~ ~~~~~~~~~~~~~~~~~~~~
...@@ -15,10 +27,10 @@ Added ...@@ -15,10 +27,10 @@ Added
Added Added
^^^^^ ^^^^^
* Added :meth:`convert_perspective_depth_to_orthogonal_depth`. :meth:`unproject_depth` assumes * Added the method :meth:`convert_perspective_depth_to_orthogonal_depth` to convert perspective depth
that the input depth image is orthogonal. The new :meth:`convert_perspective_depth_to_orthogonal_depth` images to orthogonal depth images. This is useful for the :meth:`~omni.isaac.lab.utils.math.unproject_depth`,
can be used to convert a perspective depth image into an orthogonal depth image, so that the point cloud since it expects orthogonal depth images as inputs.
can be unprojected correctly with :meth:`unproject_depth`.
0.24.13 (2024-09-08) 0.24.13 (2024-09-08)
~~~~~~~~~~~~~~~~~~~~ ~~~~~~~~~~~~~~~~~~~~
......
...@@ -182,38 +182,52 @@ def body_incoming_wrench(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg) -> tor ...@@ -182,38 +182,52 @@ def body_incoming_wrench(env: ManagerBasedEnv, asset_cfg: SceneEntityCfg) -> tor
return link_incoming_forces.view(env.num_envs, -1) return link_incoming_forces.view(env.num_envs, -1)
def grab_images( def image(
env: ManagerBasedEnv, env: ManagerBasedEnv,
sensor_cfg: SceneEntityCfg = SceneEntityCfg("tiled_camera"), sensor_cfg: SceneEntityCfg = SceneEntityCfg("tiled_camera"),
data_type: str = "rgb", data_type: str = "rgb",
convert_perspective_to_orthogonal: bool = False, convert_perspective_to_orthogonal: bool = False,
normalize: bool = True, normalize: bool = True,
) -> torch.Tensor: ) -> torch.Tensor:
"""Grab all of the latest images of a specific datatype produced by a specific camera. """Images of a specific datatype from the camera sensor.
If the flag :attr:`normalize` is True, post-processing of the images are performed based on their
data-types:
- "rgb": Scales the image to (0, 1) and subtracts with the mean of the current image batch.
- "depth" or "distance_to_camera" or "distance_to_plane": Replaces infinity values with zero.
Args: Args:
env: The environment the cameras are placed within. env: The environment the cameras are placed within.
sensor_cfg: The desired sensor to read from. Defaults to SceneEntityCfg("tiled_camera"). sensor_cfg: The desired sensor to read from. Defaults to SceneEntityCfg("tiled_camera").
data_type: The data type to pull from the desired camera. Defaults to "rgb". data_type: The data type to pull from the desired camera. Defaults to "rgb".
convert_perspective_to_orthogonal: Whether to convert perspective convert_perspective_to_orthogonal: Whether to orthogonalize perspective depth images.
depth images to orthogonal depth images. Defaults to False. This is used only when the data type is "distance_to_camera". Defaults to False.
normalize: Set to True to normalize images. Defaults to True. normalize: Whether to normalize the images. This depends on the selected data type.
Defaults to True.
Returns: Returns:
The images produced at the last timestep The images produced at the last time-step
""" """
# extract the used quantities (to enable type-hinting)
sensor: TiledCamera | Camera | RayCasterCamera = env.scene.sensors[sensor_cfg.name] sensor: TiledCamera | Camera | RayCasterCamera = env.scene.sensors[sensor_cfg.name]
# obtain the input image
images = sensor.data.output[data_type] images = sensor.data.output[data_type]
# depth image conversion
if (data_type == "distance_to_camera") and convert_perspective_to_orthogonal: if (data_type == "distance_to_camera") and convert_perspective_to_orthogonal:
images = math_utils.convert_perspective_depth_to_orthogonal_depth(images, sensor.data.intrinsic_matrices) images = math_utils.orthogonalize_perspective_depth(images, sensor.data.intrinsic_matrices)
# rgb/depth image normalization
if normalize: if normalize:
if data_type == "rgb": if data_type == "rgb":
images = images / 255 images = images.float() / 255.0
mean_tensor = torch.mean(images, dim=(1, 2), keepdim=True) mean_tensor = torch.mean(images, dim=(1, 2), keepdim=True)
images -= mean_tensor images -= mean_tensor
elif "distance_to" in data_type or "depth" in data_type: elif "distance_to" in data_type or "depth" in data_type:
images[images == float("inf")] = 0 images[images == float("inf")] = 0
return images.clone() return images.clone()
......
...@@ -376,23 +376,27 @@ class TestMathUtilities(unittest.TestCase): ...@@ -376,23 +376,27 @@ class TestMathUtilities(unittest.TestCase):
iter_old_quat_rotate_inverse(q_rand, v_rand), iter_old_quat_rotate_inverse(q_rand, v_rand),
) )
def test_depth_perspective_conversion(self): def test_orthogonalize_perspective_depth(self):
# Create a sample perspective depth image (N, H, W) """Test for converting perspective depth to orthogonal depth."""
perspective_depth = torch.tensor([[[10.0, 0.0, 100.0], [0.0, 3000.0, 0.0], [100.0, 0.0, 100.0]]]) for device in ["cpu", "cuda:0"]:
# Create a sample perspective depth image (N, H, W)
perspective_depth = torch.tensor(
[[[10.0, 0.0, 100.0], [0.0, 3000.0, 0.0], [100.0, 0.0, 100.0]]], device=device
)
# Create sample intrinsic matrix (3, 3) # Create sample intrinsic matrix (3, 3)
intrinsics = torch.tensor([[500.0, 0.0, 5.0], [0.0, 500.0, 5.0], [0.0, 0.0, 1.0]]) intrinsics = torch.tensor([[500.0, 0.0, 5.0], [0.0, 500.0, 5.0], [0.0, 0.0, 1.0]], device=device)
# Convert perspective depth to orthogonal depth # Convert perspective depth to orthogonal depth
orthogonal_depth = math_utils.convert_perspective_depth_to_orthogonal_depth(perspective_depth, intrinsics) orthogonal_depth = math_utils.orthogonalize_perspective_depth(perspective_depth, intrinsics)
# Manually compute expected orthogonal depth based on the formula for comparison # Manually compute expected orthogonal depth based on the formula for comparison
expected_orthogonal_depth = torch.tensor( expected_orthogonal_depth = torch.tensor(
[[[9.9990, 0.0000, 99.9932], [0.0000, 2999.8079, 0.0000], [99.9932, 0.0000, 99.9964]]] [[[9.9990, 0.0000, 99.9932], [0.0000, 2999.8079, 0.0000], [99.9932, 0.0000, 99.9964]]], device=device
) )
# Assert that the output is close to the expected result # Assert that the output is close to the expected result
torch.testing.assert_close(orthogonal_depth, expected_orthogonal_depth) torch.testing.assert_close(orthogonal_depth, expected_orthogonal_depth)
if __name__ == "__main__": if __name__ == "__main__":
......
...@@ -4,14 +4,15 @@ ...@@ -4,14 +4,15 @@
# SPDX-License-Identifier: BSD-3-Clause # SPDX-License-Identifier: BSD-3-Clause
import omni.isaac.lab.sim as sim_utils import omni.isaac.lab.sim as sim_utils
from omni.isaac.lab.envs.mdp.observations import grab_images
from omni.isaac.lab.managers import ObservationGroupCfg as ObsGroup from omni.isaac.lab.managers import ObservationGroupCfg as ObsGroup
from omni.isaac.lab.managers import ObservationTermCfg as ObsTerm from omni.isaac.lab.managers import ObservationTermCfg as ObsTerm
from omni.isaac.lab.managers import SceneEntityCfg from omni.isaac.lab.managers import SceneEntityCfg
from omni.isaac.lab.sensors import TiledCameraCfg from omni.isaac.lab.sensors import TiledCameraCfg
from omni.isaac.lab.utils import configclass from omni.isaac.lab.utils import configclass
from omni.isaac.lab_tasks.manager_based.classic.cartpole.cartpole_env_cfg import CartpoleEnvCfg, CartpoleSceneCfg import omni.isaac.lab_tasks.manager_based.classic.cartpole.mdp as mdp
from .cartpole_env_cfg import CartpoleEnvCfg, CartpoleSceneCfg
## ##
# Scene definition # Scene definition
...@@ -20,6 +21,8 @@ from omni.isaac.lab_tasks.manager_based.classic.cartpole.cartpole_env_cfg import ...@@ -20,6 +21,8 @@ from omni.isaac.lab_tasks.manager_based.classic.cartpole.cartpole_env_cfg import
@configclass @configclass
class CartpoleRGBCameraSceneCfg(CartpoleSceneCfg): class CartpoleRGBCameraSceneCfg(CartpoleSceneCfg):
# add camera to the scene
tiled_camera: TiledCameraCfg = TiledCameraCfg( tiled_camera: TiledCameraCfg = TiledCameraCfg(
prim_path="{ENV_REGEX_NS}/Camera", prim_path="{ENV_REGEX_NS}/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"),
...@@ -34,6 +37,8 @@ class CartpoleRGBCameraSceneCfg(CartpoleSceneCfg): ...@@ -34,6 +37,8 @@ class CartpoleRGBCameraSceneCfg(CartpoleSceneCfg):
@configclass @configclass
class CartpoleDepthCameraSceneCfg(CartpoleSceneCfg): class CartpoleDepthCameraSceneCfg(CartpoleSceneCfg):
# add camera to the scene
tiled_camera: TiledCameraCfg = TiledCameraCfg( tiled_camera: TiledCameraCfg = TiledCameraCfg(
prim_path="{ENV_REGEX_NS}/Camera", prim_path="{ENV_REGEX_NS}/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"),
...@@ -46,17 +51,22 @@ class CartpoleDepthCameraSceneCfg(CartpoleSceneCfg): ...@@ -46,17 +51,22 @@ class CartpoleDepthCameraSceneCfg(CartpoleSceneCfg):
) )
##
# MDP settings
##
@configclass @configclass
class RGBObservationsCfg: class RGBObservationsCfg:
"""Observation specifications for the MDP.""" """Observation specifications for the MDP."""
@configclass @configclass
class RGBCameraPolicyCfg(ObsGroup): class RGBCameraPolicyCfg(ObsGroup):
"""Observations for policy group.""" """Observations for policy group with RGB images."""
image = ObsTerm(func=grab_images, params={"sensor_cfg": SceneEntityCfg("tiled_camera"), "data_type": "rgb"}) image = ObsTerm(func=mdp.image, params={"sensor_cfg": SceneEntityCfg("tiled_camera"), "data_type": "rgb"})
def __post_init__(self) -> None: def __post_init__(self):
self.enable_corruption = False self.enable_corruption = False
self.concatenate_terms = True self.concatenate_terms = True
...@@ -65,22 +75,35 @@ class RGBObservationsCfg: ...@@ -65,22 +75,35 @@ class RGBObservationsCfg:
@configclass @configclass
class DepthObservationsCfg: class DepthObservationsCfg:
"""Observation specifications for the MDP."""
@configclass @configclass
class DepthCameraPolicyCfg(RGBObservationsCfg.RGBCameraPolicyCfg): class DepthCameraPolicyCfg(RGBObservationsCfg.RGBCameraPolicyCfg):
"""Observations for policy group with depth images."""
image = ObsTerm( image = ObsTerm(
func=grab_images, params={"sensor_cfg": SceneEntityCfg("tiled_camera"), "data_type": "distance_to_camera"} func=mdp.image, params={"sensor_cfg": SceneEntityCfg("tiled_camera"), "data_type": "distance_to_camera"}
) )
policy: ObsGroup = DepthCameraPolicyCfg() policy: ObsGroup = DepthCameraPolicyCfg()
##
# Environment configuration
##
@configclass @configclass
class CartpoleRGBCameraEnvCfg(CartpoleEnvCfg): class CartpoleRGBCameraEnvCfg(CartpoleEnvCfg):
"""Configuration for the cartpole environment with RGB camera."""
scene: CartpoleSceneCfg = CartpoleRGBCameraSceneCfg(num_envs=1024, env_spacing=20) scene: CartpoleSceneCfg = CartpoleRGBCameraSceneCfg(num_envs=1024, env_spacing=20)
observations = RGBObservationsCfg() observations: RGBObservationsCfg = RGBObservationsCfg()
@configclass @configclass
class CartpoleDepthCameraEnvCfg(CartpoleEnvCfg): class CartpoleDepthCameraEnvCfg(CartpoleEnvCfg):
"""Configuration for the cartpole environment with depth camera."""
scene: CartpoleSceneCfg = CartpoleDepthCameraSceneCfg(num_envs=1024, env_spacing=20) scene: CartpoleSceneCfg = CartpoleDepthCameraSceneCfg(num_envs=1024, env_spacing=20)
observations = DepthObservationsCfg() observations: DepthObservationsCfg = DepthObservationsCfg()
...@@ -176,7 +176,7 @@ class CurriculumCfg: ...@@ -176,7 +176,7 @@ class CurriculumCfg:
@configclass @configclass
class CartpoleEnvCfg(ManagerBasedRLEnvCfg): class CartpoleEnvCfg(ManagerBasedRLEnvCfg):
"""Configuration for the locomotion velocity-tracking environment.""" """Configuration for the cartpole environment."""
# Scene settings # Scene settings
scene: CartpoleSceneCfg = CartpoleSceneCfg(num_envs=4096, env_spacing=4.0) scene: CartpoleSceneCfg = CartpoleSceneCfg(num_envs=4096, env_spacing=4.0)
......
...@@ -261,7 +261,7 @@ from omni.isaac.lab.sensors import ( ...@@ -261,7 +261,7 @@ from omni.isaac.lab.sensors import (
TiledCameraCfg, TiledCameraCfg,
patterns, patterns,
) )
from omni.isaac.lab.utils.math import convert_perspective_depth_to_orthogonal_depth, unproject_depth from omni.isaac.lab.utils.math import orthogonalize_perspective_depth, unproject_depth
from omni.isaac.lab_tasks.utils import load_cfg_from_registry from omni.isaac.lab_tasks.utils import load_cfg_from_registry
...@@ -677,9 +677,8 @@ def run_simulator( ...@@ -677,9 +677,8 @@ def run_simulator(
depth = camera.data.output[data_type] depth = camera.data.output[data_type]
depth_images[data_label + "_raw"] = depth depth_images[data_label + "_raw"] = depth
if perspective_depth_predicate(data_type) and convert_depth_to_camera_to_image_plane: if perspective_depth_predicate(data_type) and convert_depth_to_camera_to_image_plane:
depth = convert_perspective_depth_to_orthogonal_depth( depth = orthogonalize_perspective_depth(
perspective_depth=camera.data.output[data_type], camera.data.output[data_type], camera.data.intrinsic_matrices
intrinsics=camera.data.intrinsic_matrices,
) )
depth_images[data_label + "_undistorted"] = depth depth_images[data_label + "_undistorted"] = depth
......
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