Pointclouds cannot be fuzed together when projecting depth map back into space

Hi, I am trying to fuse together each depth map to get one point cloud, but failed. I use camera parameters given by dataloader. Here is my code:

for i, frame in enumerate(train_dataset):
    depth = frame.depth_mask * frame.depth_map
    depth = depth[0].numpy()
    img = frame.image_rgb * frame.depth_map
    img = img.permute(1, 2, 0).numpy()

    rot, trans = frame.camera.R, frame.camera.T
    rot, trans = rot[0].numpy(), trans[0].numpy()
    extrinsic = np.eye(4)
    extrinsic[:3, :3] = rot
    extrinsic[:3, 3] = trans

    focal_length_px = frame.camera.focal_length[0].numpy() * np.array((h, w)) / 2
    principal_point = frame.camera.principal_point[0].numpy()
    principal_point_px = -1 * (principal_point - 1) * np.array((h, w)) / 2
    intrinsic = np.eye(4)
    intrinsic[[0, 1], [0, 1]] = focal_length_px
    intrinsic[:2, 2] = principal_point_px
    for u in range(w):
        for v in range(h):
            d = depth[u, v]
            if d == 0:
                continue
            coor_img = np.array((u, v, 1, 1 / d))[..., None]
            coor_world = d * np.linalg.inv(intrinsic @ extrinsic) @ coor_img
            xyz = coor_world[:3, 0]
            rgb = img[u, v]
            point = np.hstack([xyz, rgb])
            points.append(point)

Thanks a lot.

facebookresearch / co3d

Pointclouds cannot be fuzed together when projecting depth map back into space #31