Question about pointmap

[J-Oyasumi]

Hi, thanks for your excellent work!

I'm trying to obtain pointmap from base_camera in pick_cube env. I want to check if the pointmap contains the tcp(eef) point.
following https://github.com/haosulab/ManiSkill/blob/main/mani_skill/envs/utils/observations/observations.py, my code is:

positions_cam = traj['obs']['sensor_data']['base_camera']['Position'][..., :3] / 1000.0  # (T, H, W, 3) convert to meters
# Change to world coordinates
cam2world_gl = traj['obs']['sensor_param']['base_camera']['cam2world_gl'][:]  # (T, 4, 4)
R = cam2world_gl[:, :3, :3]  # (T, 3, 3)
T = cam2world_gl[:, :3, 3]
positions_world = np.einsum('tij,thwj->thwi', R, positions_cam) + T[:, None, None, :]  # (T, H, W, 3)

tcp_pos = traj['obs']['extra']['tcp_pose'][:, :3]  # (T, 3)
tcp_pos_expanded = tcp_pos[:, None, None, :]
H, W = positions_world.shape[1:3]
tcp_pos_expanded = np.tile(tcp_pos_expanded, (1, H, W, 1))
diffs = np.linalg.norm(positions_world - tcp_pos_expanded, axis=-1)  # (T, H, W)
min_dists = np.min(diffs, axis=(1, 2))  #
for t in range(len(min_dists)):
    print(f"Frame {t}: min distance from TCP to pointmap points: {min_dists[t]:.6f}")

But the min distance results are around 0.9 meters for all frames, so I'm confused about that.

Hope to get help from you.

Thanks!

[J-Oyasumi]

I check the pointmap value

  - X: min 0.598639, max 0.599863 mean 0.599311
  - Y: min 0.598185, max 0.599863 mean 0.599308
  - Z: min 0.599399, max 0.600106 mean 0.599478

All three coordinates are concentrated around 0.6.
BTW, I modify the base camera config:

sensor_camera_eye_pos [0.3, 0, 0.6] ->  [0.6, 0.6, 0.6]
sensor_cam_target_pos [-0.1, 0, 0.1] -> [0.0, 0.0, 0.35]
fov pi/2 -> 1

[J-Oyasumi]

I changed back to default base camera config and checked again.
I only kept the points corresponding to the robot, table, and cube, and calculated their position statistics. This is the result of the first frame:

Scene Points Camera Coordinates Shape: (211646, 3)
Camera Coordinate Position Statistics:
  - X: min -0.000608, max 0.000608 mean -0.000005
  - Y: min -0.000292, max 0.000465 mean -0.000019
  - Z: min -0.001115, max -0.000272 mean -0.000655
  - 
Scene Points World Coordinates Shape: (211646, 3)
World Coordinate Position Statistics:
  - X: min 0.298966, max 0.299894 mean 0.299606
  - Y: min -0.000608, max 0.000608 mean -0.000005
  - Z: min 0.599400, max 0.599873 mean 0.599477

All the points have almostly the same XYZ. while the tcp pos of the first frame:

First Frame TCP Position:
[0.01225354 0.03801134 0.18215224]

It is very different from the pointmap's value