相机坐标系到全局坐标系的几何对准

一、基本原理

将相机坐标系中的点转换到全局坐标系中，本质上是一个刚体变换的过程，涉及到旋转和平移两种基本空间操作。这个过程的核心在于找到连接这两个坐标系的几何关系，通常由一个旋转矩阵和一个平移向量共同构成，或者更简洁地由一个齐次变换矩阵来表示。理解这个对准过程，需要从空间几何关系的角度出发，逐步构建联系。

想象一下，你手中拿着一个相机，相机有其自身的坐标系，我们称之为相机坐标系。在这个坐标系中，三维空间中的一个点可以用坐标 (X_c, Y_c, Z_c) 表示，其中 Z_c 轴通常沿着相机的光轴方向指向拍摄场景，X_c 和 Y_c 轴则平行于相机的成像平面。与此同时，存在一个更大的、固定的全局坐标系（有时也称为世界坐标系），比如一个房间的坐标系或整个场景的坐标系。在这个全局坐标系中，同一个空间点有其对应的坐标 (X_w, Y_w, Z_w)。我们的目标就是找到一套数学规则，能够准确无误地将任何在相机坐标系下测量的点 (X_c, Y_c, Z_c) 转换到全局坐标系下的 (X_w, Y_w, Z_w)。

这种转换关系被称为相机相对于全局坐标系的外参。它描述的是相机在全局空间中的姿态和位置。姿态由旋转决定，它定义了相机的三个坐标轴相对于全局坐标系的三个坐标轴是如何定向的。位置由平移决定，它定义了相机坐标系的原点（通常是相机的光心）在全局坐标系中的具体位置坐标。因此，从相机坐标系到全局坐标系的转换可以清晰地分解为两个连续的几何操作：首先，将相机坐标系中的点旋转，使其坐标轴的方向与全局坐标系的坐标轴方向完全一致；然后，将旋转后的点平移，使其最终的位置对应于该点在全局坐标系中的绝对位置。

旋转操作由一个 3x3 的正交矩阵 R 表示，称为旋转矩阵。这个矩阵具有非常关键的性质：它是一个单位正交矩阵，即满足 R^T * R = I（R^T 是 R 的转置，I 是单位矩阵）且其行列式 det(R) = +1。旋转矩阵的每一列都具有明确的几何意义：第一列代表全局坐标系的 X 轴在相机坐标系下的单位方向向量；第二列代表全局坐标系的 Y 轴在相机坐标系下的单位方向向量；第三列代表全局坐标系的 Z 轴在相机坐标系下的单位方向向量。同样地，旋转矩阵的每一行则代表了相机坐标系的各个轴在全局坐标系下的单位方向向量。因此，应用旋转矩阵 R 到相机坐标系中的点 (X_c, Y_c, Z_c)，实际上就是在计算同一个点，其坐标值表达在方向与全局坐标系对齐的临时坐标系中，我们把这个旋转后的坐标记为 (X_r, Y_r, Z_r)。

平移操作由一个 3x1 的向量 T 表示，称为平移向量。这个向量 T = [T_x, T_y, T_z]^T 的几何意义非常直观：它直接给出了相机坐标系原点（光心）在全局坐标系中的坐标位置。当我们完成了旋转操作，得到了点在新方向坐标系中的坐标 (X_r, Y_r, Z_r) 后，需要再加上这个平移向量 T，才能得到该点在全局坐标系中的最终坐标 (X_w, Y_w, Z_w)。这是因为旋转操作只改变了坐标轴的方向，将点带到了一个与全局坐标系方向一致但原点仍在相机光心的坐标系中，平移操作则负责将这个点“移动”到以全局坐标系原点为参考的位置上。

将旋转和平移结合起来，就得到了完整的坐标变换公式：[X_w, Y_w, Z_w]^T = R * [X_c, Y_c, Z_c]^T + T。为了更高效地表示和计算这种变换（特别是在进行链式变换时），通常引入齐次坐标和齐次变换矩阵。在齐次坐标下，一个三维点 [X, Y, Z]^T 被表示为四维向量 [X, Y, Z, 1]^T。这样，旋转和平移就可以合并成一个单一的 4x4 变换矩阵 M。这个矩阵的结构非常清晰：左上角的 3x3 子块就是旋转矩阵 R；右上角的 3x1 子块就是平移向量 T；最后一行固定为 [0, 0, 0, 1]。利用齐次变换矩阵，坐标转换就简化为一个矩阵乘法：[X_w, Y_w, Z_w, 1]^T = M * [X_c, Y_c, Z_c, 1]^T。这个形式在计算机图形学和机器人学中应用极其广泛，因为它将复杂的空间变换统一为一个简洁的线性操作。

那么，如何实际确定这个关键的旋转矩阵 R 和平移向量 T（或者说变换矩阵 M）呢？这个过程称为相机外参标定。标定通常需要借助一个已知其在全局坐标系中精确位置的物体，称为标定物或靶标（常见的如棋盘格、圆点阵列、三维立体靶标等）。标定时，将标定物放置在相机视野中，并确保其全局坐标已知。相机拍摄标定物的图像。在图像中，我们可以检测到标定物上的一系列特征点（如棋盘格的角点）的像素坐标 (u, v)。通过相机内参（焦距、主点、畸变等，这是另一个标定过程），可以将这些像素坐标反投影到相机坐标系的三维空间，得到沿着相机光心到该特征点方向的射线，其方向可以表示为 (X_c, Y_c, Z_c)，但其深度 Z_c 是未知的（尺度因子）。关键在于，我们知道这些特征点在全局坐标系中的真实坐标 (X_w, Y_w, Z_w)。对于每一个检测到的特征点对（图像点 <-> 全局点），我们都可以建立方程：[X_w, Y_w, Z_w]^T = R * [X_c, Y_c, Z_c]^T + T。虽然 (X_c, Y_c, Z_c) 的深度未知，但它的方向是确定的（由像素坐标和内参计算得出）。通过收集足够多的（通常远多于理论最小值6个）特征点对应关系，就构成了一个方程组。求解这个方程组的目标是找到最优的 R 和 T，使得所有特征点根据变换计算出的全局坐标与其真实全局坐标之间的误差最小（通常采用最小二乘法）。求解算法（如经典的 PnP 算法：Perspective-n-Point，例如 DLT, EPnP, UPnP, 迭代法等）能够有效地从这些对应关系中估计出外参数 R 和 T。一旦外参被精确标定出来，对于任何后续相机拍摄到的图像，只要能够测量目标点在相机坐标系中的坐标（通常需要深度信息，例如来自双目视觉、结构光、ToF 或激光雷达等），就可以利用 [X_w, Y_w, Z_w]^T = R * [X_c, Y_c, Z_c]^T + T 或者对应的齐次变换矩阵乘法，将其准确地转换到全局坐标系中，实现精确的空间定位和对齐。

总之，从相机坐标系到全局坐标系的几何对准，是一个通过刚体变换（旋转加平移）实现的空间坐标转换过程。核心是确定描述相机在全局空间中姿态（旋转矩阵 R）和位置（平移向量 T）的外参数。这个过程依赖于精确的相机标定，利用已知全局坐标的特征点与它们在图像中的投影建立对应关系，并通过优化算法求解出最优的 R 和 T。最终，通过矩阵运算即可将任何相机坐标系下的点无缝转换到统一的全局坐标系中，为三维重建、机器人导航、增强现实、精密测量等应用奠定空间一致性的基础。值得注意的是，对于移动的相机或平台，这个外参可能是实时变化的（如 SLAM 系统），需要持续估计更新，但其几何变换的本质原理保持不变。

二、数理推导

1. 基础变换公式

设相机坐标系中的点为 Pc​=[Xc​,Yc​,Zc​]T，全局坐标系中的点为 Pw​=[Xw​,Yw​,Zw​]T。变换关系为：

Pw​=R⋅Pc​+T

其中：

• R 为 3×33×3 的旋转矩阵（正交矩阵，满足RTR=I 且 det(R)=1）。

• T=[Tx​,Ty​,Tz​]T 为平移向量。

---

2. 齐次坐标表示

引入齐次坐标：

P~w​=​Xw​Yw​Zw​1​​,P~c​=​Xc​Yc​Zc​1​​

变换公式简化为矩阵乘法：

]P~w​=M⋅P~c​,M=[R0T​T1​]=​r11​r21​r31​0​r12​r22​r32​0​r13​r23​r33​0​Tx​Ty​Tz​1​​

其中 M 为 4×44×4 齐次变换矩阵。

---

3. 旋转矩阵的构造

旋转矩阵 R 由三个基本旋转矩阵复合而成（以 Z-Y-X 欧拉角为例）：

• 绕 Z 轴旋转 γ：

  Rz​(γ)=​cosγsinγ0​−sinγcosγ0​001​​

• 绕 Y 轴旋转 β：

  Ry​(β)=​cosβ0−sinβ​010​sinβ0cosβ​​

• 绕 X 轴旋转 α：

  Rx​(α)=​100​0cosαsinα​0−sinαcosα​​

复合旋转矩阵：

R=Rz​(γ)⋅Ry​(β)⋅Rx​(α)

---

4. 外参标定：3D-3D 对应点法

已知 n 组对应点：{Pc,i​↔Pw,i​}i=1n​。
目标：最小化重投影误差：

min​i=1∑n​∥Pw,i​−(R⋅Pc,i​+T)∥2

求解步骤：

1. 计算重心坐标：

   μc​=n1​i=1∑n​Pc,i​,μw​=n1​i=1∑n​Pw,i​

2. 计算去中心化坐标：

   Qc,i​=Pc,i​−μc​,Qw,i​=Pw,i​−μw​

3. 构造协方差矩阵：

   H=i=1∑n​Qc,i​⋅Qw,iT​

4. 对 H 进行 SVD 分解：

   H=UΣVT

5. 计算旋转矩阵：

   R=VUT

   （若det(R)=−1，则取 R=V​100​010​00−1​​UT）

6. 计算平移向量：

   T=μw​−R⋅μc​

---

5. 外参标定：PnP 问题（3D-2D 对应点法）

已知 n 组对应点：{Pw,i​↔pi​=[ui​,vi​]T}i=1n​（pi​ 为像素坐标）。
相机模型（含内参矩阵 K）：

si​​ui​vi​1​​=K[R​T​]​Xw,i​Yw,i​Zw,i​1​​,K=​fx​00​0fy​0​cx​cy​1​​

其中 si​ 为尺度因子。

直接线性变换（DLT）求解：

1. 消去尺度因子si​：

   ⎨⎧​ui​=m3T​P~w,i​m1T​P~w,i​​vi​=m3T​P~w,i​m2T​P~w,i​​​

   其中 m1T​,m2T​,m3T​ 为投影矩阵P=K[R​T​] 的行向量。

2. 整理为线性方程组（每点提供两个方程）：

   [Xw,i​0​Yw,i​0​Zw,i​0​10​0Xw,i​​0Yw,i​​0Zw,i​​01​−ui​Xw,i​−vi​Xw,i​​−ui​Yw,i​−vi​Yw,i​​−ui​Zw,i​−vi​Zw,i​​−ui​−vi​​]​m1​m2​m3​​​=0

3. 对 n≥6 个点，构建 2n×12 矩阵A，通过 SVD 求解Ax=0（x 为 P 的元素向量）。

4. 从 P 分解出 R 和 T：

   • 利用 RQ 分解分离内参 K 和外参[R​T​]。

   • 对 P1:3,1:3​ 进行 SVD：ΣVT，取 R=UVT。

   • 平移向量：3∥T=K−1P:,4​/∥K−1P:,3​∥.

---

6. 反向变换

若需从全局坐标系转换到相机坐标系：

Pc​=RT(Pw​−T)

齐次形式：

P~c​=M−1P~w​,M−1=[RT0T​−RTT1​]

---

关键公式总结

操作	公式
正向变换	Pw​=RPc​+T
齐次变换	P~w​=[R0T​T1​]P~c​
反向变换	Pc​=RT(Pw​−T)
SVD 求解旋转	R=VUT(from H=UΣVT)
DLT 方程	[0TP~w,iT​​−P~w,iT​0T​vi​P~w,iT​−ui​P~w,iT​​]x=0

此推导完整描述了从相机坐标系到全局坐标系的几何对准过程，涵盖旋转矩阵构造、平移向量计算及外参标定的数学原理。

三、代码详解

1.相机内参标定

import numpy as np
import cv2

# ==================== 1. 相机内参（假设已标定） ====================
K = np.array([[800, 0, 320],   # fx, 0, cx
              [0, 800, 240],   # 0, fy, cy
              [0, 0, 1]])      # 内参矩阵

dist_coeffs = np.zeros(5)      # 假设无镜头畸变

2.2D-3D对应点数据

# ==================== 2. 准备3D-2D对应点数据 ====================
# 全局坐标系中的3D点（棋盘格角点，单位：mm）
# 假设棋盘格在Z=0平面上，间距30mm
obj_points = np.array([
    [0, 0, 0], [30, 0, 0], [60, 0, 0], 
    [0, 30, 0], [30, 30, 0], [60, 30, 0],
    [0, 60, 0], [30, 60, 0], [60, 60, 0]
], dtype=np.float32)

# 对应的2D图像点（通过角点检测获得）
img_points = np.array([
    [268, 189], [332, 188], [395, 187],
    [264, 223], [329, 222], [394, 220],
    [260, 258], [326, 255], [392, 252]
], dtype=np.float32)

3.PNP求解外参

# ==================== 3. 求解PnP问题获取外参 ====================
# 使用SOLVEPNP_ITERATIVE方法求解
_, rvec, tvec = cv2.solvePnP(
    objectPoints=obj_points,
    imagePoints=img_points,
    cameraMatrix=K,
    distCoeffs=dist_coeffs,
    flags=cv2.SOLVEPNP_ITERATIVE
)

print("旋转向量 (rvec):\n", rvec)
print("平移向量 (tvec):\n", tvec)

# 将旋转向量转换为旋转矩阵
R, _ = cv2.Rodrigues(rvec)
print("旋转矩阵 (R):\n", np.round(R, 4))

4.构建齐次变换矩阵

# ==================== 4. 构建齐次变换矩阵 ====================
def create_homogeneous_matrix(R, t):
    """构建4x4齐次变换矩阵"""
    M = np.eye(4)
    M[:3, :3] = R
    M[:3, 3] = t.flatten()
    return M

M_cam_to_world = create_homogeneous_matrix(R, tvec)
print("齐次变换矩阵:\n", np.round(M_cam_to_world, 4))

5.坐标转换函数

# ==================== 5. 坐标转换函数 ====================
def transform_camera_to_world(point_c, M):
    """将相机坐标系点转换到全局坐标系"""
    point_c_hom = np.append(point_c, 1)  # 转为齐次坐标
    point_w_hom = M @ point_c_hom        # 矩阵乘法
    return point_w_hom[:3]               # 转回3D坐标

def transform_world_to_camera(point_w, M):
    """将全局坐标系点转换到相机坐标系"""
    M_inv = np.linalg.inv(M)             # 求逆矩阵
    point_w_hom = np.append(point_w, 1)
    point_c_hom = M_inv @ point_w_hom
    return point_c_hom[:3]

6.转换认证

# ==================== 6. 验证转换正确性 ====================
# 测试点：相机坐标系中的点 (假设深度为500mm)
test_point_cam = np.array([0, 0, 500], dtype=np.float32)

# 转换到全局坐标系
test_point_world = transform_camera_to_world(test_point_cam, M_cam_to_world)
print(f"\n相机坐标系点 {test_point_cam} => 全局坐标系点 {np.round(test_point_world, 2)}")

# 再转换回相机坐标系
recon_point_cam = transform_world_to_camera(test_point_world, M_cam_to_world)
print(f"全局坐标系点 {np.round(test_point_world, 2)} => 相机坐标系点 {np.round(recon_point_cam, 2)}")

# 计算原始点与重建点的误差
error = np.linalg.norm(test_point_cam - recon_point_cam)
print(f"重建误差: {error:.6f} mm")

7.可视化验证

# ==================== 7. 可视化验证（可选） ====================
# 生成测试图像
image = np.zeros((480, 640, 3), dtype=np.uint8)

# 投影3D点到图像平面（验证外参）
projected_points, _ = cv2.projectPoints(
    obj_points, rvec, tvec, K, dist_coeffs)

# 绘制投影点
for i, point in enumerate(projected_points.squeeze()):
    color = (0, 255, 0) if i % 3 == 0 else (0, 0, 255)
    cv2.circle(image, tuple(point.astype(int)), 5, color, -1)
    cv2.putText(image, str(i), tuple(point.astype(int)+10), 
                cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255,255,255), 1)

cv2.imshow("Projection Verification", image)
cv2.waitKey(0)
cv2.destroyAllWindows()