The update of tsdf with dynamic scene is too slow.

[pmj110119]

If it’s not a bug, please use discussions: https://github.com/NVlabs/curobo/discussions

Please provide the below information in addition to your issue:

1. cuRobo installation mode (choose from [python, isaac sim, docker python, docker isaac sim]): docker isaac sim
2. python version: /
3. Isaac Sim version (if using): 2022.2.1

---

Hello, guys. Thanks for your great work!

I want to use RGB-D depth map with curobo, and used tsdf integrator according to issue #92. The quality of SDF mapping has improved a lot compared to 'occupancy', but the update of dynamic objects has become very slow.

As shown in the video below, the tsdf can be quickly updated when objects are detached. But when new objects are added or moved, it takes 5-10 seconds to complete the update. This delay is too long, making the robotic arm unable to be used in dynamic tasks and scenes.

20231223-200123.mp4

I am not familiar with nvblox and didn't find some adjustable parameters. Is this an inherent flaw of the nvblox algorithm? Do you have any opinions or suggestions? Thanks again.

[balakumar-s]

If you check the code here

curobo/examples/isaac_sim/realsense_reacher.py

Line 282 in c09d949

if step_index % 5 == 0.0:

We only update the esdf every 5 simulation steps. Removing that if condition will make it update at the same rate as the camera window.

[pmj110119]

If you check the code here

curobo/examples/isaac_sim/realsense_reacher.py

Line 282 in c09d949

if step_index % 5 == 0.0:

We only update the esdf every 5 simulation steps. Removing that if condition will make it update at the same rate as the camera window.

In my code I update the esdf each simulation step.

You can observe the 14th second of the video. The response of tsdf to the disappearance of the object is immediate. But when I put the object in, the response of tsdf will be particularly slow.

And the correct response time is as long as 5-10 seconds, which already includes dozens of simulation steps.

[balakumar-s]

Can you share the lines of code happening at each simulation step?

Also the line of code where you are initializing VoxelManager? It might need to be initialized with a large number of voxels if you are using a very small size for the voxels.

[pmj110119]

Sorry, I misremembered, I did use time_step to update esdf, the frequency is ten times lower than simulator-step.
But the confusing point is that the frequency of my opencv-imshow and esdf-update is the same. The update of opencv-imshow is very fast, but the esdf-update is really slow in comparison.

The code of simulation step:

    def run(self):
        while simulation_app.is_running():
            self.world.step(render=True)
            step_index = self.world.current_time_step_index

            if step_index % 10 == 0:
                # get frame data
                data = self.cam.get_frame()
                camera_to_ee = data['camera_pose']
                camera_pose = self.get_camera_pose(camera_to_ee)
                camera_pose = format_pose.from_transforma_mat(camera_pose)


                # update esdf
                voxels = self.planner.update_rgbd(
                    depth=data['depth'],
                    intrinsics=data['intrinsics'],
                    camera_pose=camera_pose,
                )



                # visualization -- simulator
                self.visual_voxels(voxels)


                # visualization -- opencv-window
                depth_image = data['raw_depth']
                color_image = data['rgb']
                depth_colormap = cv2.applyColorMap(
                    cv2.convertScaleAbs(depth_image, alpha=100), cv2.COLORMAP_JET
                )
                images = np.hstack((color_image, depth_colormap))
                cv2.namedWindow("Align Example", cv2.WINDOW_NORMAL)
                cv2.imshow("Align Example", images)
                key = cv2.waitKey(1)
                

        print("finished program")

        simulation_app.close()

The code of VoxelManager

    self.voxel_viewer = VoxelManager(2000, size=0.03)

    def visual_voxels(self, voxels):
        if voxels.shape[0] > 0:
            voxels = voxels[voxels[:, 2] > 0]
            if voxels.shape[0]>2000:
                voxels = voxels[::(voxels.shape[0]//2000), :]
            voxels = voxels.cpu().numpy()
            self.voxel_viewer.update_voxels(voxels[:, :3])

[balakumar-s]

Can you print the size of voxels? If it's more than 2000, then they won't be rendered as voxelmanager was created with a buffer of 2000.

[balakumar-s]

1. Can you share the code inside self.planner.update_rgbd().
2. What is the voxel size you are using in nvblox?

[pmj110119]

0. Yes, in order to improve the speed, I did a simple downsampling when voxel_size > 2000. Although the voxels becomes sparse, the esdf can still be visualized evenly.

   if voxels.shape[0]>2000:
               voxels = voxels[::(voxels.shape[0]//2000), :]

1. Code inside self.planner.update_rgbd:

   def update_rgbd(self, depth, intrinsics, camera_pose):
       data_camera = CameraObservation(
               depth_image=depth, intrinsics=intrinsics, pose=camera_pose
           )
       data_camera = data_camera.to(device=self.tensor_args.device)
   
       self.world_model.add_camera_frame(data_camera, "world")
       self.world_model.process_camera_frames("world", False)
       torch.cuda.synchronize()
       self.world_model.update_blox_hashes()
       voxel_size = 0.03
       bounding = Cuboid("t", dims=[1, 1, 1.0], pose=[0, 0, 0, 1, 0, 0, 0])
       voxels = self.world_model.get_voxels_in_bounding_box(bounding, voxel_size)
   
       return voxels

2. The voxel size used in nvblox is 0.01. I used to use 0.002 and the results are similar.

[balakumar-s]

You are missing a decay layer call:

Add this in the first line of self.planner.update_rgbd()
world_model.decay_layer("world")
This lets nvblox know that you are moving forward in time and should account for dynamic changes in the scene.

[pmj110119]

I remember that when adding decay before, I would get an error like : only 'occupancy integrator' supports occupancy decay, so I removed it.

My world_cfg:

        self.world_cfg = WorldConfig.from_dict(
            {
                "blox": {
                    "world": {
                        "pose": [0, 0, 0, 1, 0, 0, 0],
                        "integrator_type": "tsdf",
                        "voxel_size": 0.01,
                    }
                }
            }
        )

[balakumar-s]

We updated to nvblox 0.0.5 in the latest release so you should not see this error.

Can you check now and report if you are seeing the error?

[pmj110119]

My docker image was built two weeks ago, and I just realized that you updated it within two weeks.

I need to recreate the environment and test again. Thank you very much for your patient help!!!

[pmj110119]

I changed to newest version, but unfortunately the error still occur. And the nvblox version is 0.0.5.