《三维点如何映射到图像像素？—

引言

以三维投影介绍大多比较分散，不少小伙伴再面对诸多的坐标系转换中容易弄混，特别是再写代码的时候可能搞错，所有这篇文章帮大家完整的梳理3D视觉中的投影变换的全流程，一文弄清楚这个过程，帮助大家搞清坐标系转换。

想象你正站在一个空房间里，脚下是地板，面前是墙。

在纸上画一个直角坐标系，标记原点为墙角（左下角），X向右，Y向前，Z向上。
相机位于[2, 4, 1.5]位置。
在[4, 2, 1.5]处点一个点——这是我们要投影到相机图像的三维点。

接下来，我会带你一步步推导：这个点如何变成相机照片中的一个像素。

第一章：从3D到2D的四大步骤

在计算机视觉和三维重建领域，将三维空间中的点映射到二维图像平面是相机成像过程的核心。这个过程可以抽象为一个投影模型（Projection Model），它描述了如何将真实世界中的点通过一系列变换最终落在图像传感器上，成为我们看到的像素点。

整个过程可以分为四个关键步骤：

步骤一：世界坐标系 → 相机坐标系（刚体变换）

三维空间中的点通常以**世界坐标系（World Coordinate System）表示，记为 $P_w = (X_w, Y_w, Z_w)^T$ ，为了模拟相机的视角，我们需要将其转换到相机坐标系（Camera Coordinate System）中。这一步是通过刚体变换（Rigid Transformation）**完成的，包括旋转和平移操作：

投影公式如下：

$\mathbf{P}_c = \mathbf{R} \cdot \mathbf{P}_w + \mathbf{t}$

中：

R 是旋转矩阵（3x3）；
t 是平移向量（3x1）；
P_c 是点在相机坐标系下的表示。

也可以用齐次坐标写成一个变换矩阵：

$\begin{bmatrix} \mathbf{P}_c \\ 1 \end{bmatrix} = \begin{bmatrix} \mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} \cdot \begin{bmatrix} \mathbf{P}_w \\ 1 \end{bmatrix}$

步骤二：相机坐标系 → 归一化图像坐标系（透视投影）

接下来，我们将三维相机坐标系中的点通过透视投影（Perspective Projection）映射到二维的归一化图像坐标系中。这一步模拟了光线穿过针孔相机的过程。

设相机焦距为 f，则投影公式如下：

$\begin{bmatrix} x \\ y \end{bmatrix} = \frac{f}{Z_c} \cdot \begin{bmatrix} X_c \\ Y_c \end{bmatrix}$

这里的 (x, y) 表示归一化图像平面上的坐标，与实际像素无关。

步骤三：归一化图像坐标 → 像素坐标（内参变换）

为了得到最终的像素坐标，还需要考虑相机的内部参数，例如像素尺寸、主点偏移等。这些信息被包含在一个称为相机内参矩阵（Intrinsic Matrix）的矩阵中：

$\mathbf{K} = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}$

其中：

f_x, f_y 是以像素为单位的焦距；
(c_x, c_y) 是图像主点（principal point），通常是图像中心。

于是，归一化坐标 (x, y) 到像素坐标 (u, v) 的变换为：

$\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \mathbf{K} \cdot \begin{bmatrix} x \\ y \\ 1 \end{bmatrix}$

步骤四：像素坐标 → 图像坐标系（图像裁剪与翻转）

最后一步是将像素坐标 (u, v) 映射到图像的实际坐标系统中。通常图像坐标原点位于左上角，而之前计算的 (u, v) 是浮点数，需要进行取整或插值处理。

此外，还要注意图像可能会有缩放、裁剪、翻转等后处理操作，这会影响最终的像素位置。

第二章：从世界坐标系到相机坐标系的刚体变换详解

——坐标系定义、旋转矩阵与平移矩阵的构建全过程

一、相机坐标系（CCS）的定义与物理意义

在计算机视觉中，相机坐标系（Camera Coordinate System, CCS） 是以相机为中心建立的右手坐标系。其定义如下：

原点（Oₐ）：位于相机光心（光学中心）；
Z轴（Zₐ）：指向相机拍摄方向（即光轴方向）；
X轴（Xₐ）：指向图像右侧；
Y轴（Yₐ）：指向图像下方，满足右手系规则：Xₐ × Yₐ = Zₐ。

二、世界坐标系（WCS）与相机坐标系的关系

我们考虑一个具体例子：

世界坐标系（WCS）：
- 原点位于房间角落；
- X轴向右；
- Y轴向前；
- Z轴向上。
相机位置：
- 在 WCS 中的位置为：t =[2, 4, 1.5]ᵀ；
- 表示相机在房间中位于 x=2, y=4, z=1.5 的位置（单位：米）；
相机朝向：
- 光轴（Zₐ）指向 WCS 的 −Y 方向；
- 即相机背对世界前方（Y轴正方向），面朝后方。

这个设定常见于室内监控场景中，例如相机安装在房间中央，面朝墙壁。

三、刚体变换公式

将一个三维点 P_w 从世界坐标系转换到相机坐标系的过程如下：

四、分步构建变换过程

步骤 1：平移变换（Translation）

目标：将世界坐标点“移动”到以相机为原点的相对位置。

公式：

$\mathbf{P}_{rel} = \mathbf{P}_w - \mathbf{t}$

示例：

P_w = np.array([4, 2, 1.5]) # 修改后的世界点位置

t = np.array([2, 4, 1.5])

则：

$\mathbf{P}_{rel} = \begin{bmatrix}4 - 2 \\ 2- 4 \\ 1.5 - 1.5 \end{bmatrix} = \begin{bmatrix} 2 \\ -2 \\ 0 \end{bmatrix}$

物理意义：该点相对于相机的位置是向右 2 米、向前 -2 米、下方 0 米(相机没有旋转前)。

步骤 2：旋转变换（Rotation）

目标：将相对向量 PrelPrel 转换到相机坐标系的方向中去。

（1）确定相机坐标系的三个单位向量

根据设定，相机光轴指向 WCS 的 −Y 方向：

Zₐ（光轴方向）：[0, −1, 0]
Xₐ（右侧方向）：选择与 Zₐ 垂直的世界 X 轴方向：[1, 0, 0]
Yₐ（下方方向）：由叉积得到：Yₐ = Zₐ × Xₐ = [0, 0, −1]

这三个向量构成了相机坐标系的基底。

（2）构造旋转矩阵 R

将上述三个单位向量作为列向量，组成旋转矩阵：

$\mathbf{R} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & -1 & 0 \end{bmatrix}$

解释每一列含义：

第1列：世界 X 轴在 CCS 中的方向；
第2列：世界 Y 轴在 CCS 中的方向；
第3列：世界 Z 轴在 CCS 中的方向。

五、完整变换计算

继续使用上面的例子：

import numpy as np

P_w = np.array([2, 3, 0.5])
t = np.array([1, 2, 1.5])

R = np.array([
    [1, 0, 0],
    [0, 0, -1],
    [0, -1, 0]
])

P_rel = P_w - t
P_c = R @ P_rel

结果：

$\mathbf{P}_c = \begin{bmatrix} 2 \\ 0 \\ 2 \end{bmatrix}$

解释：

Xc = 2：点在相机右侧 2 米；
Yc = 0：点在相机下方 0 米（Y轴向下）；
Zc = 2：点在相机前方2米。

六、为什么是 −Y 方向而不是 +Y？

这是由以下两个因素决定的：

1. 相机坐标系的定义要求

为了保持右手系结构（Xₐ × Yₐ = Zₐ），如果 Xₐ 对齐世界 X 轴、Yₐ 对齐世界 Z 轴，则 Zₐ 必须指向世界 −Y。

2. 图像坐标系的一致性

在图像坐标系中，通常定义：

u 轴（列）向右；
v 轴（行）向下；

这与相机坐标系中 Yₐ 向下一致，因此相机朝向 −Y 可以自然地映射到图像平面。

第三章：旋转矩阵 R 的数学构建与相机坐标系的构建原理详解

在第二章中，我们已经介绍了如何从世界坐标系（WCS）到相机坐标系（CCS）进行刚体变换，并通过一个具体示例演示了平移和旋转的具体计算过程。本章将深入讲解旋转矩阵 R 的数学构造方法、其物理意义以及相机坐标系构建背后的几何逻辑。

一、旋转矩阵 R 的数学定义与性质

1.1 基本定义

旋转矩阵是一个 3×3 的正交矩阵，满足以下两个重要性质：

正交性

$\mathbf{R}^T \mathbf{R} = \mathbf{I}$

行列式为 1：

$\det(\mathbf{R}) = 1$

这两个条件保证了旋转操作只改变方向而不改变长度或形状。

1.2 旋转矩阵的列向量解释

设世界坐标系中的三个标准基向量为：

$\mathbf{e}_x = [1,0,0]^T,\quad \mathbf{e}_y = [0,1,0]^T,\quad \mathbf{e}_z = [0,0,1]^T$

而相机坐标系的三个单位基向量为：

$\mathbf{x}_c,\quad \mathbf{y}_c,\quad \mathbf{z}_c$

则旋转矩阵 R 可以表示为：

$\mathbf{R} = \begin{bmatrix} | & | & | \\ \mathbf{x}_c & \mathbf{y}_c & \mathbf{z}_c \\ | & | & | \end{bmatrix}$ $\mathbf{z}_c = [0, 0, 1]^T$

换句话说，旋转矩阵的每一列是相机坐标系在世界坐标系下的方向向量。

✅ 通俗理解：旋转矩阵 R 描述了“世界坐标轴在相机坐标系中的方向”。

1.3 构造 R 的通用步骤

步骤 1：确定相机光轴方向（Zₐ）

这是最重要的一步，决定了相机的拍摄方向。例如：

若希望相机朝向世界 −Y 方向，则：

$\mathbf{z}_c = [0, -1, 0]^T$

若希望相机朝向世界 +Z 方向，则：

$\mathbf{z}_c = [0, 0, 1]^T$

步骤 2：选择 Xₐ 向量（与 Zₐ 垂直）

通常优先对齐世界坐标系中的某个轴（如 X 轴），但必须确保与 Zₐ 垂直：

设定：

$\mathbf{x}_c = [1, 0, 0]^T \quad (\text{if } \mathbf{z}_c \text{ not parallel to } X \text{ axis})$

或者使用 Gram-Schmidt 正交化法调整方向。

步骤 3：由叉积得到 Yₐ 向量

根据右手系规则

$\mathbf{y}_c = \mathbf{z}_c \times \mathbf{x}_c$

这确保了整个坐标系保持右手结构。

步骤 4：归一化所有向量

确保每个向量都是单位向量：

$\mathbf{x}_c \leftarrow \frac{\mathbf{x}_c}{\|\mathbf{x}_c\|},\quad \text{deal with in the same way } \mathbf{y}_c, \mathbf{z}_c$

步骤 5：构造旋转矩阵 R

将三个单位向量作为列向量排列成矩阵：

二、相机坐标系为什么这么构建？

相机坐标系的设计并不是随意的，而是为了满足以下几个关键目标：

2.1 图像坐标的自然映射

图像坐标系通常定义如下：

u 轴（列方向）→ 相机右侧；
v 轴（行方向）→ 相机下方；
原点位于图像左上角。

因此，相机坐标系的 Xₐ 向右、Yₐ 向下可以自然地映射到图像坐标。

2.2 保持右手系结构

三维空间建模和渲染中广泛使用右手系，这样便于统一处理旋转、投影等操作。如果 Yₐ 指向上方，则会导致图像坐标的行方向与 Yₐ 方向相反，增加转换复杂度。

2.3 简化后续投影模型

相机坐标系的 Zₐ 轴指向拍摄方向，使得后续的透视投影公式简洁且直观：

$x = f \cdot \frac{X_c}{Z_c}, \quad y = f \cdot \frac{Y_c}{Z_c}$

其中 f 是焦距。

三、构建相机坐标系的完整数学流程示例

假设：

相机位置：t=[1,2,1.5]T，表示相机在世界坐标系中的位置为 x=1, y=2, z=1.5。
相机光轴指向：世界 −Y 方向（即面朝房间后方）。

步骤 1：确定 Zₐ

Zₐ 是相机坐标系的 Z 轴方向，表示相机的拍摄方向。在这个例子中

✅ 相机朝向世界 −Y 方向，因此：

$\mathbf{z}_c = [0, -1, 0]^T$

这是单位向量，无需归一化。

📌 注意：Zₐ 并不是相机的位置向量，而是指向相机“看的方向”。在这里，“看向 −Y”意味着相机背对世界 +Y 轴。

步骤 2：确定 Xₐ

Xₐ 应该与 Zₐ 垂直，并且尽可能与世界坐标系的某个轴对齐（通常选 X 或 Y），以便简化后续计算和理解。

在这个例子中，我们可以选择：

$\mathbf{x}_c = [1, 0, 0]^T$

因为：

它是单位向量；
与 Zₐ 垂直（点积为零）：

步骤 3：由叉积得 Yₐ

$\mathbf{y}_c = \mathbf{z}_c \times \mathbf{x}_c = \begin{vmatrix} \mathbf{i} & \mathbf{j} & \mathbf{k} \\ 0 & -1 & 0 \\ 1 & 0 & 0 \end{vmatrix} = (0 \cdot 0 - 0 \cdot 0)\mathbf{i} - (0 \cdot 0 - 0 \cdot 1)\mathbf{j} + (0 \cdot 0 - (-1) \cdot 1)\mathbf{k} = [0, 0, 1]^T$

步骤 4：归一化各向量（此处已是单位向量）

步骤 5：构造旋转矩阵 R

$\mathbf{R} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{bmatrix}$

第四章：相机坐标系 → 归一化图像坐标系（透视投影）

在第三章中，我们完成了从世界坐标系到相机坐标系的变换，即通过旋转和平移将一个点 PwPw 转换为相机坐标 Pc=[Xc,Yc,Zc]TPc=[Xc,Yc,Zc]T。

本章将继续这一流程，介绍如何将三维空间中的点 投影到二维归一化图像平面，完成：

从 相机坐标系（CCS） 到 归一化图像坐标系（Normalized Image Coordinates） 的变换。

一、什么是归一化图像坐标？

归一化图像坐标是针孔相机模型下的中间图像坐标系统。它是一个无单位的二维坐标系，原点位于图像中心，Z 轴与相机光轴重合。

它的定义如下：

设相机焦距为 f，则对于相机坐标系下的点 Pc=[Xc,Yc,Zc]T，其对应的归一化图像坐标 (x,y)为：

$x = f * \frac{X_c}{Z_c}$

$y = f * \frac{Y_c}{Z_c}$

这个过程称为透视投影（Perspective Projection），它模拟了真实相机成像的基本原理 —— 远小近大。

✅ 注意：这里的 (x,y)是归一化的坐标，还没有考虑图像分辨率、主点偏移等参数。这是后续章节会讨论的内容。

二、沿用之前的示例进行推导

示例设定：

相机位置： $\mathbf{t} = [2, 4, 1.5]^T$

旋转矩阵： $\mathbf{R} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & -1 & 0 \end{bmatrix}$
焦距： f=1

该旋转矩阵表示相机的光轴指向世界 −Z 方向（向下看），Yₐ 向下，Xₐ 向右。

举例说明：选取几个3D点进行投影

我们在相机坐标系中选取了五个点，它们分别是：

点编号	X_c	Y_c	Z_c
P0	0.5	0.5	1.5
P1	-0.5	0.3	2
P2	0.2	-0.4	3
P3	0	0	5
P4	0.8	0.8	0.8

这些点都位于相机前方（Zₐ > 0），因此可以被正确投影到图像平面上。

投影计算示例（以 P0 为例）

对于点 P0：

Xc=0.5,Yc=0.5,Zc=1.5

代入公式得：

$x = \frac{0.5}{1.5} = 0.333$ $y = \frac{0.5}{1.5} = 0.333$

所以归一化图像坐标为： $(x, y) = (0.333, 0.333)$

类似地，我们可以计算其他点的归一化图像坐标。

投影结果汇总（部分）

点编号	X_c	Y_c	Z_c	x	y
P0	0.5	0.5	1.5	0.333	0.333
P1	-0.5	0.3	2	-0.25	0.15
P2	0.2	-0.4	3	0.067	-0.133
P3	0	0	5	0	0
P4	0.8	0.8	0.8	1.0	1.0

可以看到，随着 Z 值变小（靠近相机），投影点变得更大；而远离相机的点（如 P3）则更接近图像中心。

三、物理意义解释

参数	数值范围	物理意义
Z_c > 0	所有点都在相机前方	可以被正常投影到图像上
Z_c < 0	未出现在示例中	表示点在相机背后，不会出现在图像中
Z_c = 0	未出现	表示点在相机正前方无穷远处，无法投影
x/y 接近 0	如 P3	表示点位于图像中心附近
x/y 绝对值较大	如 P4	表示点靠近图像边缘或超出视野

四、注意事项

当 Zc=0时，表示点在相机正前方无穷远处，无法投影；
当 Zc<0时，点在相机背后，不会出现在图像中；
实际图像坐标还需结合内参矩阵（焦距、主点、畸变）进一步映射；
本章只讨论理想情况下的归一化图像坐标。

第五章：归一化图像坐标 → 像素坐标（内参变换）

在第四章中，我们完成了从相机坐标系到归一化图像坐标系的投影过程，得到了一个无单位的二维坐标 (x,y)。

本章将介绍如何将这个归一化图像坐标进一步转换为像素坐标 (u,v)，这一步是通过引入**相机内参矩阵（Intrinsic Camera Matrix）**来实现的。

一、什么是相机内参矩阵？

相机内参矩阵 KK 是一个 3×3的上三角矩阵，它包含了以下物理参数：

参数	物理意义
fx,fy	焦距（以像素为单位），通常 fx=fy=f，但在非正方形像素时可以不同
cx,cy	主点（Principal Point），即图像中心在像素坐标中的位置（通常为图像分辨率的一半）
s	图像的斜切因子（Skew Factor），表示像素是否正交排列，默认为 0

完整的相机内参矩阵形式如下：

$\mathbf{K} = \begin{bmatrix} f_x & s & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}$

实际使用中，大多数情况下 s=0s=0，所以常简化为：

$\mathbf{K} = \begin{bmatrix} f & 0 & c_x \\ 0 & f & c_y \\ 0 & 0 & 1 \end{bmatrix}$

二、归一化图像坐标 → 像素坐标的公式

设归一化图像坐标为 (x,y)，则对应的像素坐标 (u,v)可由下式计算：

$\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \mathbf{K} \cdot \begin{bmatrix} x \\ y \\ 1 \end{bmatrix}$

展开后得到：

这个过程称为 “内参变换”，它不依赖于相机的位置或朝向，只与相机本身的成像特性有关。

三、延续最开始的例子进行推导

示例设定：

相机位置： $\mathbf{t} = [2, 4, 1.5]^T$
点的世界坐标： $\mathbf{P}_w = [4, 2, 1.5]^T$
旋转矩阵（沿用之前例子）： $\mathbf{R} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & -1 & 0 \end{bmatrix}$

步骤 1：计算相机坐标

先求相对坐标：

$\mathbf{P}_{rel} = \mathbf{P}_w - \mathbf{t} = [2, -2, 0]^T$

再应用旋转：

$\mathbf{P}_c = \mathbf{R} \cdot \mathbf{P}_{rel} = [2, 0, 2]^T$

所以：

$X_c = 2,\quad Y_c = 0,\quad Z_c = 2$

步骤 2：透视投影（归一化图像坐标）

焦距设为 f=200

$x = f \cdot \frac{X_c}{Z_c} = 200 \cdot \frac{2}{2} = 200$

$y = f \cdot \frac{Y_c}{Z_c} = 200 \cdot \frac{0}{2} = 0$

所以归一化图像坐标为：

$(x, y) = (200, 0)$

步骤 3：假设一个内参矩阵

设相机分辨率为 640×480，则主点为：

$c_x = 320,\quad c_y = 240$

构建内参矩阵：

$\mathbf{K} = \begin{bmatrix} f_x & s & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix}$

$\mathbf{K} = \begin{bmatrix} 200& 0 & 320 \\ 0 & 200& 240 \\ 0 & 0 & 1 \end{bmatrix}$

步骤 4：计算像素坐标

代入公式：

$u = f \cdot x + c_x = 200+ 320 = 520$

$v = f \cdot y + c_y = 0 + 240 = 240$

所以最终像素坐标为：

✅ 注意事项

若像素坐标超出图像范围则该点不在图像视野范围内；
调整焦距 ff或相机姿态可以改变点是否出现在图像中；
如果 Zc<0，即使计算出像素坐标也无法显示，因为该点在相机背后。

四、参数总结与物理意义

参数	单位	物理意义
ff（焦距）	像素	控制图像放大倍数，越大视角越小，物体显得更大
cx,cycx,cy	像素	主点偏移量，通常是图像中心，控制图像原点位置
Xc,Yc,ZcXc,Yc,Zc	米或任意单位	点在相机坐标系下的三维坐标
x,y	无量纲	归一化图像坐标，反映点相对于图像中心的位置
u,v	像素	最终图像坐标，可用于绘制或识别目标位置

第六章：更复杂的案例 —— 相机与世界坐标系存在夹角（完整流程详解）

在本章中，我们将以一个更复杂的案例为例，详细推导从世界坐标 → 像素坐标的完整变换流程。这个案例的特点是：

✅ 相机姿态不平行于世界坐标轴 —— 即相机与世界坐标系之间存在夹角。

🎯 示例设定

1. 点的世界坐标：

P_w = np.array([4, 2, 1.5])

表示某个物体在世界坐标中的位置。

2. 相机参数

（1）相机位置（平移向量）：

t = np.array([2, 4, 1.5])

相机位于世界坐标系中的 (2, 4, 1.5) 位置。

（2）原始旋转矩阵（将世界坐标转换为相机坐标）：

R_original = np.array([
    [1, 0, 0],
    [0, 0, -1],
    [0, -1, 0]
])

3. 新增旋转：绕相机 Yₐ 轴向右旋转 30°

我们希望让相机绕其自身的 Z轴偏转 30°(X->Y)，从而看到右侧空间。

在三维空间中，绕 Y 轴 旋转一个角度 θ 的标准旋转矩阵为：

$\mathbf{R}_y(\theta) = \begin{bmatrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{bmatrix}$

物理意义：

表示绕 Y 轴旋转角度 θ；
相当于“转头”动作：向左或向右看；
X 和 Z 坐标发生变化，Y 不变

绕 X 轴旋转（Pitch）一个角度 θ 的标准旋转矩阵为：

$\mathbf{R}_x(\theta) = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta \\ 0 & \sin\theta & \cos\theta \end{bmatrix}$

物理意义：

表示绕 X 轴旋转角度 θ；
相当于“点头”动作：向上或向下看；
Y 和 Z 坐标发生变化，X 不变。

绕 Z 轴旋转（Roll）一个角度 θ 的标准旋转矩阵为：

$\mathbf{R}_z(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix}$

物理意义：

表示绕 Y 轴旋转角度 θ；
相当于“转头”动作：向左或向右看；
X 和 Z 坐标发生变化，Y 不变。

构造绕相机 zₐ 轴的旋转矩阵：

theta = np.radians(30)
R_y_camera =  np.array([
    [np.cos(theta), -np.sin(theta), 0],
    [np.sin(theta), np.cos(theta), 0],
    [0, 0, 1]
])

代入数值近似为：

array([[ 0.8660254, -0.5      ,  0.       ],
       [ 0.5      ,  0.8660254,  0.       ],
       [ 0.       ,  0.       ,  1.       ]])

4. 新的旋转矩阵（合成后的相机姿态）

我们将新增的局部旋转应用到原旋转上，得到新的整体旋转矩阵，实际应该过程中，会很容易忽略掉原始相机坐标系R_original从而导致出错：

R_new = R_y_camera @ R_original

代入计算得：

array([[ 0.8660254,  0.       ,  0.5      ],
       [ 0.5      ,  0.       , -0.8660254],
       [ 0.       , -1.       ,  0.       ]])

🔁 完整变换流程（公式代码格式）

✅ 步骤 1：世界坐标 → 相机坐标

$\mathbf{P}_c = \mathbf{R}_{\text{new}} \cdot (\mathbf{P}_w - \mathbf{t})$

代入数值：

P_rel = P_w - t  
P_c = R_new @ P_rel

结果为：

p_c = array([1.73205081, 1.        , 2.        ])

即：

$X_c = 1.732,\quad Y_c = 1,\quad Z_c =2$

X_w = np.array([1, 0, 0])  # 世界X轴
Y_w = np.array([0, 1, 0])  # 世界Y轴
Z_w = np.array([0, 0, 1])  # 世界Z轴

t = np.array([2, 4, 1.5])  # 新相机位置（Y值更大，更靠后）
P_w = np.array([4, 2, 1.5])  
# 旋转矩阵（光轴 Z_c = -Y_w）
R = np.array([
    [1, 0,0],
    [0, 0, -1],
    [0, -1, 0]
])

theta = np.radians(30)
R_z = np.array([
    [np.cos(theta), -np.sin(theta), 0],
    [np.sin(theta), np.cos(theta), 0],
    [0, 0, 1]
])

R = R_z @ R


# 转换到相机坐标系
P_c = R @ (P_w - t)