世界坐标系、相机坐标系、图像坐标系、像素坐标系之间的转换代码

以下是世界坐标系、相机坐标系、图像坐标系、像素坐标系之间的转换代码：

1. 世界坐标系转相机坐标系：

import numpy as np

def world_to_camera(world_point, R, t):
    """
    世界坐标系转相机坐标系
    :param world_point: 世界坐标系中的点，shape=(3,)
    :param R: 相机姿态的旋转矩阵，shape=(3, 3)
    :param t: 相机姿态的平移向量，shape=(3,)
    :return: 相机坐标系中的点，shape=(3,)
    """
    camera_point = np.dot(R, world_point) + t
    return camera_point

2. 相机坐标系转世界坐标系：

import numpy as np

def camera_to_world(camera_point, R, t):
    """
    相机坐标系转世界坐标系
    :param camera_point: 相机坐标系中的点，shape=(3,)
    :param R: 相机姿态的旋转矩阵，shape=(3, 3)
    :param t: 相机姿态的平移向量，shape=(3,)
    :return: 世界坐标系中的点，shape=(3,)
    """
    world_point = np.dot(np.linalg.inv(R), camera_point - t)
    return world_point

3. 相机坐标系转图像坐标系：

import numpy as np

def camera_to_image(camera_point, K):
    """
    相机坐标系转图像坐标系
    :param camera_point: 相机坐标系中的点，shape=(3,)
    :param K: 相机内参矩阵，shape=(3, 3)
    :return: 图像坐标系中的点，shape=(2,)
    """
    image_point = np.dot(K, camera_point[:3])
    image_point = image_point[:2] / image_point[2]
    return image_point

4. 图像坐标系转像素坐标系：

def image_to_pixel(image_point):
    """
    图像坐标系转像素坐标系
    :param image_point: 图像坐标系中的点，shape=(2,)
    :return: 像素坐标系中的点，shape=(2,)
    """
    pixel_point = np.round(image_point).astype(int)
    return pixel_point

5. 像素坐标系转图像坐标系：

def pixel_to_image(pixel_point):
    """
    像素坐标系转图像坐标系
    :param pixel_point: 像素坐标系中的点，shape=(2,)
    :return: 图像坐标系中的点，shape=(2,)
    """
    image_point = pixel_point.astype(float)
    return image_point