Problem with images I get from cameras

Hello,

I use an environment composed of a robot and a wrist mounted camera. This environment is cloned several times and to avoid to increase the size of the memory, the same camera is moved between the different instances of the environment to take the pictures. Thus, each time I need to take a picture, I get the pose of the link on which the camera should be fixed on the robot and then I set the pose of the real camera there.

When using the headless mode, the images taken by the camera seem good. However, when headless is false, then sometimes, the camera seems to be sometimes at the wrong location. It seems that the image I received is the one corresponding to another timestep than the one I’m supposed to get.

Does someone have an hint on what could be going on ?

1 Like

Hi @alempereur - Can you share the code as well to debug further?

Hi, It’s hard to reproduce the problem since our codebase is complex. Here is a code that reproduce a similar problem:

import os

from PIL import Image
from omni.isaac.kit import SimulationApp
headless = True
kit = SimulationApp({"headless": headless})

import numpy as np
from omni.isaac.core import World
from omni.isaac.core.prims import XFormPrim
import omni.isaac.core.utils.prims as prim_utils
import omni.replicator.core as rep
from pxr import UsdGeom
from omni.isaac.core.objects import DynamicCuboid
import omni


def get_and_save_image(annotators, name):
    rgb = annotators["rgb"].get_data()
    rgb = rgb[:, :, :3].astype(np.uint8)
    rgb_img = Image.fromarray(rgb, "RGB")
    rgb_img.save(f"images/{'headless' if headless else 'render'}/{name}.png")


def define_usd_camera_attributes(sensor_prim):
    params = {
        "focal_length": 3.67,
        "horizontal_aperture": 4.148322976111168,
        "vertical_aperture": 3.0403275478185177,
        "clipping_range": (0.01, 4),

    }
    # set parameters for camera
    for param_name, param_value in params.items():
        set_param_attr(sensor_prim, param_name, param_value)


def to_camel_case(snake_str: str, to= "cC") -> str:
    """Converts a string from snake case to camel case.
    Args:
        snake_str (str): A string in snake case (i.e. with '_')
        to (Optional[str], optional): Convention to convert string to. Defaults to "cC".
    Raises:
        ValueError: Invalid input argument `to`, i.e. not "cC" or "CC".
    Returns:
        str: A string in camel-case format.
    """
    # check input is correct
    if to not in ["cC", "CC"]:
        msg = "to_camel_case(): Choose a valid `to` argument (CC or cC)"
        raise ValueError(msg)
    # convert string to lower case and split
    components = snake_str.lower().split("_")
    if to == "cC":
        # We capitalize the first letter of each component except the first one
        # with the 'title' method and join them together.
        return components[0] + "".join(x.title() for x in components[1:])
    else:
        # Capitalize first letter in all the components
        return "".join(x.title() for x in components)

def set_param_attr(sensor_prim, param_name: str, param_value):
        # convert to camel case (CC)
        param_name = to_camel_case(param_name, to="CC")
        # get attribute from the class
        param_attr = getattr(sensor_prim, f"Get{param_name}Attr")
        # set value
        # note: We have to do it this way because the camera might be on a different layer (default cameras are on session layer),
        #   and this is the simplest way to set the property on the right layer.
        omni.usd.utils.set_prop_val(param_attr(), param_value)

if __name__ == "__main__":
    import carb
    physics_dt = 1 / 240
    gravity_magnitude = 9.81
    os.makedirs(f"images/{'headless' if headless else 'render'}", exist_ok=True)
    carb.settings.get_settings().set_bool("/app/runLoops/main/rateLimitEnabled", True)
    carb.settings.get_settings().set_int("/app/runLoops/main/rateLimitFrequency", int(1 / physics_dt))
    carb.settings.get_settings().set_int("/persistent/simulation/minFrameRate", int(1 / physics_dt))
    world = World(physics_dt=physics_dt, rendering_dt=physics_dt, stage_units_in_meters=1.0)
    world.get_physics_context().set_gravity(-gravity_magnitude)
    world.scene.add_default_ground_plane()
    world.scene.enable_bounding_boxes_computations()
    rep.orchestrator.run()

    prim_path = "/World/camera"
    sensor_prim = UsdGeom.Camera(prim_utils.define_prim(prim_path, "Camera"))
    define_usd_camera_attributes(sensor_prim)
    sensor_xform = XFormPrim(prim_path)
    rp = rep.create.render_product(prim_path, resolution=(224, 171))

    annotators = dict()
    annotators["rgb"] = rep.AnnotatorRegistry.get_annotator("rgb")
    annotators["rgb"].attach([rp])

    prim = world.scene.add(
        DynamicCuboid(
            prim_path="/World/cube",  # The prim path of the cube in the USD stage
            name="cube",  # The unique name used to retrieve the object from the scene later on
            position=np.array([0, 0, 0.025]),  # Using the current stage units which is in meters by default.
            scale=np.array([0.05, 0.05, 0.05]),  # most arguments accept mainly numpy arrays.
            color=np.array([1, 0, 0]),  # RGB channels, going from 0-1
        ))

    world.reset()
    # Place the camera at the origin and look at the cube
    sensor_xform.set_world_pose(np.array([0., 0, 0.2]), np.array([1, 0, 0., 0]))
    for _ in range(100):
        world.step()
    get_and_save_image(annotators, "init_pose")
    # Move the camera up
    sensor_xform.set_world_pose(np.array([0., 0., 0.8]), np.array([1, 0, 0, 0]))
    world.step()
    # The image is not yet the good one
    get_and_save_image(annotators, f"after_1_step")
    world.step()
    # The image is not yet the good one
    get_and_save_image(annotators, f"after_2_steps")
    world.step()
    # The image is good
    get_and_save_image(annotators, f"after_3_steps")

    kit.close()

In this code, the camera is moved from one position to a new position and 3 simulation steps are necessary to update the camera image I got from the camera whereas in theory, only one should be needed.

Maybe my issue is linked to this one: Camera images are always delayed 2 rendering frames

1 Like

@alempereur @rthaker This is the exactly same issue I had. Camera images are always delayed! This issue makes whole camera related simulation inaccurate.

1 Like

Hi there,

Thank you for reporting this and for the repro script. As a workaround you could insert a few world.render() calls before calling get_data() to make sure the annotator will have the latest data.

The issue is being investigated and we will try to provide a cleaner solution for the next release.

This topic was automatically closed 14 days after the last reply. New replies are no longer allowed.