三维目标检测框的中心点坐标

[GXQ0527]

作者，您好。经检测得出的目标三维框中心是以相机坐标系为准，但是现在想获取激光雷达坐标系下的三维目标检测框中心，不知从何解决，看了kitti_dataset.py代码，发现里面有很多注解关于project a 3D box into camera coordinate，但是还是不知如何解决，希望作者给予帮助，期待您的回复。谢谢！

[WeijingShi]

Hi @GXQ0527, Apologize for the mess in kitti_dataset.py. For now, I hope the following code makes the coordinate system more clear.

def get_clean_calibration(self, frame_idx):
    calib_file = join(self._calib_dir, self._file_list[frame_idx]) + ".txt"
    with open(calib_file, "r") as f:
        calib = {}
        for line in f:
            fields = line.split(" ")
            matrix_name = fields[0].rstrip(":")
            matrix = np.array(fields[1:], dtype=np.float32)
            calib[matrix_name] = matrix

    calib["P2"] = calib["P2"].reshape(3, 4)

    # rectified_camera_points = rectified_ref_camera_points + t (offset between camera and ref_camera)
    # rectified_image_points = intrinsics * rectified_camera_points
    #                        = intrinsics * (rectified_ref_camera_points + t) = P2 * rectified_ref_camera_points
    # take out the intrinsics and t from P2:
    intrinsics = calib["P2"][:, :3]
    assert np.isclose(intrinsics[0, 1], 0)
    assert np.isclose(intrinsics[1, 0], 0)
    assert np.isclose(intrinsics[2, 0], 0)
    assert np.isclose(intrinsics[2, 1], 0)
    assert np.isclose(intrinsics[2, 2], 1)
    assert np.isclose(intrinsics[0, 0], intrinsics[1, 1])

    t = np.linalg.inv(intrinsics).dot(calib["P2"][:, [3]])

    # rectified_ref_camera_points = rotation * ref_camera_points
    # ref_camera_points = velo_to_cam * velo_points
    # rectified_camera_points = rectified_ref_camera_points + t
    #                         = rotation * ref_camera_points + t
    #                         = rotation * velo_to_cam * velo_points + t
    #                         = velo_to_rect_cam * velo_points
    # Compute velo_to_rect_cam:
    velo_to_cam = np.vstack([calib["Tr_velo_to_cam"].reshape(3, 4), [0, 0, 0, 1]])
    rotation = calib["R0_rect"].reshape(3, 3)
    rotation_and_t = np.eye(4)
    rotation_and_t[:3, :3] = rotation
    rotation_and_t[3, :3] = t[:, 0]
    velo_to_rect_cam = rotation_and_t.dot(velo_to_cam)

    # Summery
    # rectified_image_points = intrinsics * rectified_camera_points
    # rectified_camera_points = velo_to_rect_cam * velo_points
    return intrinsics, velo_to_rect_cam

Using this function, the transformation from camera coordinates to Velodyne, is simply velo_object_center = np.linalg.inv(velo_to_rect_cam).dot(cam_object_center).

Hope it helps.