import torch


def coords_world_to_cam(scene_coords, gt_coords, gt_poses):
    """
    Transform the scene coordinates to camera coordinates.
    @param scene_coords           [B, 3, N] Predicted scene coords tensor.
    @param gt_coords              [B, 3, N] Ground-truth scene coords tensor.
    @param gt_poses               [B, 4, 4] cam-to-world matrix.
    @return camera_coords         [B, 3, N] camera coords tensor corresponding to scene_coords.
    @return target_camera_coords  [B, 3, N] camera coords tensor corresponding to gt_coords.
    """
    gt_pose_inv = gt_poses.inverse()[:, 0:3, :]  # [B, 3, 4], world to camera matrix
    ones = torch.ones((scene_coords.size(0), 1, scene_coords.size(2))).cuda()

    scene_coords_ = torch.cat([scene_coords, ones], dim=1)  # [B, 4, N]
    gt_coords_ = torch.cat([gt_coords, ones], dim=1)  # [B, 4, N]

    camera_coords = torch.bmm(gt_pose_inv, scene_coords_)  # [B, 3, N] = [B, 3, 4] * [B, 4, N]
    target_camera_coords = torch.bmm(gt_pose_inv, gt_coords_)  # [B, 3, N] = [B, 3, 4] * [B, 4, N]

    return camera_coords, target_camera_coords