代码&&手推相机内参K

小孔成像：

1.1 坐标系

世界3D-->相机3D-->图像2D-->像素2D

• 世界坐标系（物体在实际世界中的坐标系）3D，一般以现实中的某个物体为参照中心点

• 相机坐标系：在光心O处,3D

• 图像坐标系：在物理成像平面, 2D ,by 相似三角形，中心点在O'

• 像素坐标系：在像素平面，对相机坐标系进行了缩放和平移,2D， by 透视投影，中心点在左上角

小孔成像：本质上就是物体在相机坐标系的点3D到图像坐标系的2D转化过程。

1.2 内参：

成像平面坐标系到像素坐标系的变换（缩放和原点平移），焦距f

相机内参（intrinsic parameters）是指相机本身的内部属性，包括焦距、主点位置等参数。它描述了相机从三维空间到二维图像的映射关系。具体来说，相机内参包括以下几个参数：

1. 焦距（f）：表示相机镜头对焦时物体到成像平面的距离。

2. 主点位置（cx, cy）：表示成像平面上的光学中心位置。

3. 图像畸变参数：由于相机镜头存在畸变，需要通过这些参数对图像进行校正。（光线经过相机镜头后发生折射和散射导致像平面上不同位置的像素大小和形状不一样）

4. 像素大小：表示成像平面上一个像素所代表的实际长度。

相机内参通常通过标定（calibration）过程来获得，标定方法包括棋盘格法、自然场景法等。在计算机视觉领域中，相机内参是很重要的参数，用于研究相机成像原理、进行三维重建、姿态估计等任务。

1.3 外参：

相机外参（extrinsic parameters）是指相机在世界坐标系中的位置和方向。具体来说，它描述了相机坐标系与世界坐标系之间的转换关系。

通常情况下，我们可以用一个三维变换矩阵来表示相机的外参，这个矩阵包括旋转矩阵和平移向量。

旋转矩阵描述了相机坐标系与世界坐标系之间的旋转关系，

平移向量描述了相机坐标系原点在世界坐标系中的位置。

相机外参对于计算机视觉中的很多问题都非常重要，比如三维重建、深度估计、姿态估计等。一般来说，我们需要通过相机内参、匹配点以及某些先验知识来求解相机的外参。

相机坐标-->像素坐标：

import numpy as np

def project_to_pixel(point, K, image_width, image_height):
    # 将点 XYZ 转换为相机坐标系下的齐次坐标形式
    point_homogeneous = np.array([point[0], point[1], point[2], 1.0])

    # 计算相机坐标系下的投影值
    projection = np.dot(K, point_homogeneous)

    # 归一化平面上的坐标值
    normalized_coordinates = projection[:2] / projection[2]

    # 像素平面上的坐标值
    u = normalized_coordinates[0] * image_width
    v = normalized_coordinates[1] * image_height

    return u, v

# 输入相机内参矩阵 K
K = np.array([[fx, 0, cx],
              [0, fy, cy],
              [0, 0, 1]])

# 输入点 XYZ 的坐标值
point = [X, Y, Z]

# 输入图像的宽度和高度
image_width = ...
image_height = ...

# 计算投影值 UV
u, v = project_to_pixel(point, K, image_width, image_height)

print("投影后的 UV 值：", u, v)

def project3Dto2D(points_local, center_local, pose, K, height, width):
    relative_points = points_local - center_local             # (0,0,0), 物体在物体坐标系下的点云的坐标
    homogeneous_points = np.ones((4,relative_points.shape[0]))
    homogeneous_points[:3,:] = relative_points.transpose()

    camera_points = np.matmul(pose, homogeneous_points)  # 物体在cam坐标系下的点云的坐标

    image_points = np.matmul(K, camera_points[:3,:]) # 相机坐标系下的点云坐标投影到图像平面

    assert np.min(image_points[2]) > 0

    image_points[0] = image_points[0]/image_points[2] # 将投影后的图像坐标除以深度，得到归一化坐标。
    image_points[1] = image_points[1]/image_points[2]

    pixel_points = np.round(image_points[:2,:]) # 对归一化坐标取整，得到像素坐标。

    mask1 = pixel_points[0,:] > -1            # 根据像素坐标的范围进行剪裁，即将超出图像范围的点云剔除。
    mask2 = pixel_points[1,:] > -1
    mask3 = pixel_points[0,:] < width
    mask4 = pixel_points[1,:] < height

    mask = mask1&mask2&mask3&mask4
    selected_index = np.where(mask)
    pixel_points = np.take(pixel_points, selected_index[0], axis=1)
    depth = np.take(image_points[2,:], selected_index[0])
    
    return pixel_points.astype(int), depth

像素坐标-->相机坐标

cx, cy, fx, fy = 323, 238, 616, 616
x = (u - cx) / fx
y = (v - cy) / fy
x = x * z
y = y * z