How to get pointcloud of cameras?

import numpy as np
import os
from omni.isaac.kit import SimulationApp
experience = f'{os.environ["EXP_PATH"]}/omni.isaac.sim.python.gym.kit'
simulation_app = SimulationApp(
                {"headless": False, "physics_gpu": 0}, experience=experience
            )
from omni.isaac.core import World
from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.core.robots import Robot
from omni.isaac.core.prims import RigidPrim
from omni.isaac.sensor.scripts.camera import Camera
        
        
rendering_dt=1.0 / 60.0
backend="numpy"
sim_params=None
        
_world = World(
    stage_units_in_meters=1.0, rendering_dt=rendering_dt, backend=backend, sim_params=sim_params, device="cpu")
_world.scene.add_default_ground_plane()


# load the scene
robot_usd_file = "/home/user/Downloads/shadow_hand_description/ur5e_shadow/ur5e_shadow_new.usd"
add_reference_to_stage(usd_path=robot_usd_file, prim_path="/robot")
robot = _world.scene.add(Robot(prim_path="/robot/ur5e_shadow", name="robot",position=np.array([0.5,0,0]),orientation=np.array([1,0,0,0])))

can_usd_file = "/home/user/Downloads/shadow_hand_description/ur5e_shadow/_02_master_chef_can.usd"
add_reference_to_stage(usd_path=can_usd_file, prim_path="/object")
can = _world.scene.add(RigidPrim(prim_path="/object",name="can",position=np.array([0.5,0.5,0]),orientation=np.array([1,0,0,0])))
camera = _world.scene.add(Camera(prim_path="/camera",position=np.array([0.5,0,2]),orientation=np.array([1,0,0,0]),resolution=(680,680)))
camera.initialize()
for i in range(5):
    print("resetting...")
    _world.reset()

    for i in range(500):
        _world.step(render=True)
        point_cloud = camera.get_pointcloud()
        # print(camera.supported_annotators)
        print(point_cloud)

simulation_app.close()

2024-03-21 12:27:24 [13,262ms] [Warning] [omni.isaac.sensor.scripts.camera] [get_depth][/camera] WARNING: Annotator ‘distance_to_image_plane’ not found. Available annotators: dict_keys([‘rendering_time’, ‘rendering_frame’, ‘rgba’]). Returning None

Hi there,

because the camera sensor uses the annotator depth data to construct the pointcloud, you will need to add camera.add_distance_to_image_plane_to_frame() to your script (after initialize()):

camera.initialize()
camera.add_distance_to_image_plane_to_frame()

I add camera.add_distance_to_image_plane_to_frame() to my script.

import os
from omni.isaac.kit import SimulationApp
experience = f'{os.environ["EXP_PATH"]}/omni.isaac.sim.python.gym.kit'
simulation_app = SimulationApp(
                {"headless": False, "physics_gpu": 0}, experience=experience
            )
from omni.isaac.core import World
from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.core.robots import Robot
from omni.isaac.core.prims import RigidPrim
from omni.isaac.sensor.scripts.camera import Camera
        
        
rendering_dt=1.0 / 60.0
backend="numpy"
sim_params=None
        
_world = World(
    stage_units_in_meters=1.0, rendering_dt=rendering_dt, backend=backend, sim_params=sim_params, device="cpu")
_world.scene.add_default_ground_plane()


# load the scene
robot_usd_file = "/home/user/Downloads/shadow_hand_description/ur5e_shadow/ur5e_shadow_new.usd"
add_reference_to_stage(usd_path=robot_usd_file, prim_path="/robot")
robot = _world.scene.add(Robot(prim_path="/robot/ur5e_shadow", name="robot",position=np.array([0.5,0,0]),orientation=np.array([1,0,0,0])))

can_usd_file = "/home/user/Downloads/shadow_hand_description/ur5e_shadow/_02_master_chef_can_new.usd"
add_reference_to_stage(usd_path=can_usd_file, prim_path="/object")
can = _world.scene.add(RigidPrim(prim_path="/object/_02_master_chef_can",name="can",position=np.array([0.5,0.5,0]),orientation=np.array([1,0,0,0])))
camera = _world.scene.add(Camera(prim_path="/camera",position=np.array([0.5,0,2]),orientation=np.array([1,0,0,0]),resolution=(680,680)))
_world.reset()
camera.initialize()
camera.add_distance_to_image_plane_to_frame()
for i in range(500):
    _world.step(render=True)
    point_cloud = camera.get_pointcloud()
    # print(camera.supported_annotators)
    print()

simulation_app.close()

2024-03-22 07:49:32 [9,779ms] [Warning] [omni.fabric.plugin] No source has valid data array=0x1e990190 usdValid=0 cpuValid=0
2024-03-22 07:49:32 [9,881ms] [Warning] [carb] Client omni.physx.plugin has acquired [omni::physics::schema::IUsdPhysics v1.1] 100 times. Consider accessing this interface with carb::getCachedInterface() (Performance warning)
2024-03-22 07:49:32 [9,968ms] [Warning] [omni.isaac.sensor.scripts.camera] [get_depth][/camera] WARNING: Annotator ‘distance_to_image_plane’ contains no data. Returning None
2024-03-22 07:49:32 [9,969ms] [Warning] [omni.isaac.sensor.scripts.camera] [get_pointcloud][/camera] WARNING: Unable to get depth. Returning None
2024-03-22 07:49:32 [10,028ms] [Warning] [omni.isaac.sensor.scripts.camera] [get_depth][/camera] WARNING: Annotator ‘distance_to_image_plane’ contains no data. Returning None
2024-03-22 07:49:32 [10,028ms] [Warning] [omni.isaac.sensor.scripts.camera] [get_pointcloud][/camera] WARNING: Unable to get depth. Returning None
2024-03-22 07:49:32 [10,068ms] [Error] [omni.kit.app._impl] [py stderr]: /home/user/.local/share/ov/pkg/isaac_sim-2023.1.1/exts/omni.isaac.sensor/omni/isaac/sensor/scripts/camera.py:1467: RuntimeWarning: invalid value encountered in multiply
self._backend_utils.transpose_2d(homogenous) * self._backend_utils.expand_dims(depth, 0),
/home/user/.local/share/ov/pkg/isaac_sim-2023.1.1/exts/omni.isaac.sensor/omni/isaac/sensor/scripts/camera.py:1467: RuntimeWarning: invalid value encountered in multiply
self._backend_utils.transpose_2d(homogenous) * self._backend_utils.expand_dims(depth, 0),
2024-03-22 07:49:32 [10,079ms] [Error] [omni.kit.app._impl] [py stderr]: /home/user/.local/share/ov/pkg/isaac_sim-2023.1.1/exts/omni.isaac.core/omni/isaac/core/utils/numpy/maths.py:13: RuntimeWarning: invalid value encountered in matmul
return np.matmul(matrix_a, matrix_b)

This works on my side, can you test if it works for you as well?
I removed the parts with the local files I don’t have access to, and added the camera.add_distance_to_image_plane_to_frame() part:

import numpy as np
import os
from omni.isaac.kit import SimulationApp
simulation_app = SimulationApp({"headless": False})

from omni.isaac.core import World
from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.core.robots import Robot
from omni.isaac.core.prims import RigidPrim
from omni.isaac.sensor.scripts.camera import Camera
        
        
rendering_dt=1.0 / 60.0
backend="numpy"
sim_params=None
        
_world = World(
    stage_units_in_meters=1.0, rendering_dt=rendering_dt, backend=backend, sim_params=sim_params, device="cpu")
_world.scene.add_default_ground_plane()

camera = _world.scene.add(Camera(prim_path="/camera",position=np.array([0.5,0,2]),orientation=np.array([1,0,0,0]),resolution=(680,680)))
camera.initialize()
camera.add_distance_to_image_plane_to_frame()
for i in range(5):
    print("resetting...")
    _world.reset()

    for i in range(500):
        _world.step(render=True)
        point_cloud = camera.get_pointcloud()
        # print(camera.supported_annotators)
        print(point_cloud)

simulation_app.close()

Probably because of NaN values in the depth sensor, which in turn probably appear becaues of inf depth distance.

Would using the pointcloud annotator direclty be an alternative solution for you? If yes, make sure you use the includeUnlabelled flag to include data of unclabaled assets as well:
rep.AnnotatorRegistry.get_annotator("pointcloud", init_params={"includeUnlabelled": True})

Since the pointcloud data is in world frame, here is how you can access it in camera frame:

import asyncio

import numpy as np
import omni.replicator.core as rep

RESOLUTION = (3, 3)

plane = rep.create.plane(position=(0, 0, -2), scale=(10, 10, 1))
sphere = rep.create.sphere(position=(0, 0, 1), semantics=[("class", "sphere")])
cam = rep.create.camera(position=(0, 0, 7), look_at=(0, 0, 0))
rp = rep.create.render_product(cam, resolution=RESOLUTION)

# pc_annot = rep.AnnotatorRegistry.get_annotator("pointcloud")
# If the 'includeUnlabelled' param is set to True, the point cloud data will include unlabelled objects as well (e.g. the plane)
pc_annot = rep.AnnotatorRegistry.get_annotator("pointcloud", init_params={"includeUnlabelled": True})
cam_annot = rep.AnnotatorRegistry.get_annotator("CameraParams")

pc_annot.attach(rp)
cam_annot.attach(rp)


async def run_example_async():
    await rep.orchestrator.step_async()

    pc_data = pc_annot.get_data()
    cam_data = cam_annot.get_data()

    cam_transform_row_major = cam_data["cameraViewTransform"]
    cam_transform_row_major = np.array(cam_transform_row_major).reshape((4, 4))
    print(f"cam_transform_row_major={cam_transform_row_major}")

    pc_world_frame = pc_data["data"]
    print(f"pc_world_frame={pc_world_frame}")

    # Homogenize the point cloud data (x, y, z) -> (x, y, z, 1) for multiplication with the camera transform
    pc_homogenized = np.hstack((pc_world_frame, np.ones((pc_world_frame.shape[0], 1))))
    # Transform to camera frame (no need to transpose the transformation matrix since it is column-major)
    pc_camera_frame = pc_homogenized @ cam_transform_row_major
    # De-homogenize the point cloud data (x, y, z, 1) -> (x, y, z)
    pc_camera_frame = pc_camera_frame[:, :3]
    print(f"pc_camera_frame={pc_camera_frame}")


asyncio.ensure_future(run_example_async())

Thanks for you,but I want to know how to fix the problem above.


Why is the shape of pc_camera_frame (9,3)?
it means only to contain 9 points???

I set the resolution in the example to 3x3

3x3 was just as an example to be able to visualize the data in the terminal, in your previous example you had your resolution set to resolution=(680,680)

Thanks for you!

2024-03-25 10:01:47 [9,387ms] [Warning] [omni.fabric.plugin] No source has valid data array=0x28d06810 usdValid=0 cpuValid=0
2024-03-25 10:01:47 [9,395ms] [Warning] [omni.fabric.plugin] No source has valid data array=0x28d6f068 usdValid=0 cpuValid=0
Module omni.replicator.core.ogn.python._impl.nodes.OgnSemanticSegmentation load on device ‘cuda:0’ took 3.12 ms
2024-03-25 10:01:50 [12,468ms] [Warning] [carb] [Plugin: omni.spectree.delegate.plugin] Module /home/user/.local/share/ov/pkg/isaac_sim-2023.1.1/kit/exts/omni.usd_resolver/bin/libomni.spectree.delegate.plugin.so remained loaded after unload request
2024-03-25 10:01:50 [12,471ms] [Warning] [omni.graph.core.plugin] Node type name ‘’ is missing the unique namespace
2024-03-25 10:01:50 [12,471ms] [Warning] [omni.stageupdate.plugin] Deprecated: direct use of IStageUpdate callbacks is deprecated. Use IStageUpdate::getStageUpdate instead.
2024-03-25 10:01:50 [12,471ms] [Warning] [omni.graph.core.plugin] Could not find category ‘Replicator:Annotators’ for removal
2024-03-25 10:01:50 [12,471ms] [Warning] [omni.graph.core.plugin] Could not find category ‘Replicator:Annotators’ for removal
2024-03-25 10:01:50 [12,471ms] [Warning] [omni.graph.core.plugin] Could not find category ‘Replicator:Annotators’ for removal
2024-03-25 10:01:50 [12,471ms] [Warning] [omni.graph.core.plugin] Could not find category ‘Replicator:Annotators’ for removal
2024-03-25 10:01:50 [12,471ms] [Warning] [omni.graph.core.plugin] Could not find category ‘Replicator:Annotators’ for removal
2024-03-25 10:01:50 [12,471ms] [Warning] [omni.graph.core.plugin] Could not find category ‘Replicator:Annotators’ for removal
2024-03-25 10:01:50 [12,471ms] [Warning] [omni.graph.core.plugin] Could not find category ‘Replicator:Annotators’ for removal
2024-03-25 10:01:50 [12,471ms] [Warning] [omni.graph.core.plugin] Could not find category ‘Replicator:Annotators’ for removal
2024-03-25 10:01:50 [12,471ms] [Warning] [omni.graph.core.plugin] Could not find category ‘Replicator:Annotators’ for removal
2024-03-25 10:01:50 [12,471ms] [Warning] [omni.graph.core.plugin] Could not find category ‘Replicator:Annotators’ for removal
2024-03-25 10:01:50 [12,471ms] [Warning] [omni.graph.core.plugin] Could not find category ‘Replicator:Annotators’ for removal
2024-03-25 10:01:50 [12,471ms] [Warning] [omni.graph.core.plugin] Could not find category ‘Replicator:Core’ for removal
2024-03-25 10:01:50 [12,474ms] [Error] [omni.graph.core.plugin] N4omni5graph4core4NodeE::getPointerFromHandle - handle 47 not found
2024-03-25 10:01:50 [12,475ms] [Warning] [carb.audio.context] 1 contexts were leaked
2024-03-25 10:01:50 [12,500ms] [Warning] [carb] Recursive unloadAllPlugins() detected!
2024-03-25 10:01:50 [12,505ms] [Warning] [omni.core.ITypeFactory] Module /home/user/.local/share/ov/pkg/isaac_sim-2023.1.1/kit/exts/omni.activity.core/bin/libomni.activity.core.plugin.so remained loaded after unload request.

I want to know what this error means.

Could you provide the repro snippet leading to this error?