阶段三：坐标变换和多视角

这一阶段把单相机 point map 扩展到双相机：先构造两台已知相机，再把各自相机坐标系下的点云变换到同一个世界坐标系中，最后合并为带颜色的 XYZRGB 点云。

实验文件：

• pointmap_stage3.py
• stage3_merged_pointcloud.png
• stage3_merged_xyzrgb.npy

运行方式：

pip install numpy matplotlib scipy
python pointmap_stage3.py

实际测试结果：

SE(3) checks passed.

camera 1 max |Z_w - plane_z|: 0.0
camera 2 max |Z_w - plane_z|: 4.44e-16

valid pixels camera 1: 76800
valid pixels camera 2: 76800
camera-1 points visible in camera 2: 72615

All stage-three tests passed.

一、先固定变换约定

这是阶段三最重要的事情。

本文统一使用：

$$T_{dst \leftarrow src}$$

表示把 src 坐标系中的点变换到 dst 坐标系：

$$\tilde{P}_{dst}=T_{dst \leftarrow src}\tilde{P}_{src}$$

例如：

$$\tilde{P}_{w}=T_{w \leftarrow c_1}\tilde{P}_{c_1}$$

代码中命名为：

T_c1_to_w

含义是：

camera 1 coordinates -> world coordinates

类似地：

• T_w_to_c1
• T_c2_to_w
• T_w_to_c2
• T_c1_to_c2

不要使用模糊名称：

• pose
• extrinsic
• transform
• camera_matrix

这些名字没有告诉你变换方向，很容易在多视角里把矩阵用反。

二、SE(3) 矩阵的结构

刚体变换可以写成：

$$T_{dst \leftarrow src}= \begin{bmatrix} R_{dst \leftarrow src} & t_{dst \leftarrow src}\\ 0 & 1 \end{bmatrix}$$

对列向量：

$$P_{dst}=R_{dst \leftarrow src}P_{src}+t_{dst \leftarrow src}$$

其中：

• R 表示坐标轴方向变化。
• t 表示源坐标系原点在目标坐标系中的位置。

特别是：

$$T_{w \leftarrow c}= \begin{bmatrix} R_{w \leftarrow c} & t_{w \leftarrow c}\\ 0 & 1 \end{bmatrix}$$

其中：

$$t_{w \leftarrow c}$$

就是相机中心的世界坐标。

三、实现 transform_points()

点数组一般存储为：

points_src.shape == (..., 3)

例如：

• (N, 3)
• (H, W, 3)
• (B, H, W, 3)

数学公式采用列向量：

$$P_{dst}=RP_{src}+t$$

但 NumPy 中每个点通常存成行向量，因此实现为：

$$P_{dst}^{row}=P_{src}^{row}R^T+t$$

def transform_points(T_dst_src, points_src):
    """
    把 src 坐标系中的点变换到 dst 坐标系。

    数学约定：
        p_dst_h = T_dst_src @ p_src_h

    points_src 可以是：
        (N, 3)
        (H, W, 3)
        (..., 3)
    """
    T_dst_src = np.asarray(T_dst_src, dtype=np.float64)
    points_src = np.asarray(points_src, dtype=np.float64)

    if T_dst_src.shape != (4, 4):
        raise ValueError(
            f"T must have shape (4, 4), got {T_dst_src.shape}"
        )

    if points_src.shape[-1] != 3:
        raise ValueError(
            f"points_src must have shape (..., 3), "
            f"got {points_src.shape}"
        )

    R_dst_src = T_dst_src[:3, :3]
    t_dst_src = T_dst_src[:3, 3]

    points_dst = (
        points_src @ R_dst_src.T
        + t_dst_src
    )

    return points_dst

这个写法会自动广播平移向量，所以既可以处理普通点云，也可以处理 H x W x 3 的 point map。

为什么 NumPy 代码里是 R.T？

假设只有一个点：

point_src.shape == (3,)

数学上写成列向量：

$$P_{dst}=RP_{src}$$

但 NumPy 中这个点存成行向量：

point_src = [x, y, z]

所以对应写法是：

point_dst = point_src @ R.T

下面两种写法对单个点等价：

point_dst = R @ point_src
point_dst = point_src @ R.T

但对于：

points.shape == (H, W, 3)

第二种写法更自然，因为它可以直接对最后一个维度做矩阵乘法，不需要把整张 point map 先改成列向量形式。

四、实现 SE(3) 的逆

已知：

$$P_{dst}=RP_{src}+t$$

反解：

$$P_{src}=R^T(P_{dst}-t)$$

因此：

$$T_{src \leftarrow dst}= \begin{bmatrix} R^T & -R^Tt\\ 0 & 1 \end{bmatrix}$$

实现：

def invert_se3(T_dst_src):
    """
    输入 T_dst_src，返回 T_src_dst。
    """
    T_dst_src = np.asarray(
        T_dst_src,
        dtype=np.float64,
    )

    R_dst_src = T_dst_src[:3, :3]
    t_dst_src = T_dst_src[:3, 3]

    T_src_dst = np.eye(
        4,
        dtype=np.float64,
    )

    T_src_dst[:3, :3] = R_dst_src.T
    T_src_dst[:3, 3] = (
        -R_dst_src.T @ t_dst_src
    )

    return T_src_dst

不要直接认为“逆变换就是平移取负”。只有纯平移、没有旋转时才有：

$$t^{-1}=-t$$

一般情况下必须是：

$$t^{-1}=-R^Tt$$

五、验证逆变换

假设：

T_c1_to_c2
T_c2_to_c1

互为逆矩阵，则应该满足：

$$T_{c_1 \leftarrow c_2}T_{c_2 \leftarrow c_1}=I$$

以及：

$$T_{c_2 \leftarrow c_1}T_{c_1 \leftarrow c_2}=I$$

测试：

I = np.eye(4)

np.testing.assert_allclose(
    T_c2_to_c1 @ T_c1_to_c2,
    I,
    atol=1e-10,
)

np.testing.assert_allclose(
    T_c1_to_c2 @ T_c2_to_c1,
    I,
    atol=1e-10,
)

同时还要对实际三维点做往返测试：

points_c1 = np.array([
    [0.0, 0.0, 2.0],
    [0.5, -0.2, 3.0],
    [-0.3, 0.4, 4.0],
])

points_c2 = transform_points(
    T_c1_to_c2,
    points_c1,
)

points_c1_recovered = transform_points(
    T_c2_to_c1,
    points_c2,
)

np.testing.assert_allclose(
    points_c1_recovered,
    points_c1,
    atol=1e-10,
)

矩阵检查和点检查都应该保留。矩阵检查能发现逆矩阵是否正确，点检查能发现 transform_points() 的行列向量约定是否一致。

六、验证变换复合顺序

设：

$$T_{b \leftarrow a}$$

把 a 坐标变到 b 坐标，

$$T_{c \leftarrow b}$$

把 b 坐标变到 c 坐标。

那么：

$$P_b=T_{b \leftarrow a}P_a$$ $$P_c=T_{c \leftarrow b}P_b$$

代入：

$$P_c=T_{c \leftarrow b}T_{b \leftarrow a}P_a$$

因此：

$$T_{c \leftarrow a}=T_{c \leftarrow b}T_{b \leftarrow a}$$

注意矩阵顺序与自然语言阅读顺序相反：先 a -> b，再 b -> c，矩阵写成：

T_a_to_c = T_b_to_c @ T_a_to_b

测试：

points_c_two_steps = transform_points(
    T_b_to_c,
    transform_points(
        T_a_to_b,
        points_a,
    ),
)

T_a_to_c = T_b_to_c @ T_a_to_b

points_c_one_step = transform_points(
    T_a_to_c,
    points_a,
)

np.testing.assert_allclose(
    points_c_two_steps,
    points_c_one_step,
    atol=1e-10,
)

这是防止矩阵乘法顺序错误的关键测试。

七、构造两台相机

令相机 1 定义世界坐标系：

T_c1_to_w = np.eye(4)

因此：

$$P_w=P_{c_1}$$

相机 2 做平移和旋转。设相机 2 的世界坐标为：

$$C_{w,2}= \begin{bmatrix} 0.50\\ 0.02\\ 0.08 \end{bmatrix}$$

并绕世界 y 轴旋转 -7 deg：

R_c2_to_w = rotation_y(
    np.deg2rad(-7.0)
)

t_c2_to_w = np.array([
    0.50,
    0.02,
    0.08,
])

T_c2_to_w = np.eye(4)
T_c2_to_w[:3, :3] = R_c2_to_w
T_c2_to_w[:3, 3] = t_c2_to_w

绕 y 轴的旋转：

def rotation_y(angle):
    c = np.cos(angle)
    s = np.sin(angle)

    return np.array([
        [ c, 0.0, s],
        [0.0, 1.0, 0.0],
        [-s, 0.0, c],
    ])

这两台相机位置不同、朝向不同，但都能观察世界前方的同一平面。

八、计算相机之间的相对变换

已知：

T_c1_to_w
T_c2_to_w

先求：

T_w_to_c1 = invert_se3(T_c1_to_w)
T_w_to_c2 = invert_se3(T_c2_to_w)

从相机 1 到相机 2：

$$P_{c_2}=T_{c_2 \leftarrow w}P_w$$

而：

$$P_w=T_{w \leftarrow c_1}P_{c_1}$$

所以：

$$P_{c_2}=T_{c_2 \leftarrow w}T_{w \leftarrow c_1}P_{c_1}$$

即：

$$T_{c_2 \leftarrow c_1}=T_{c_2 \leftarrow w}T_{w \leftarrow c_1}$$

代码：

T_c1_to_c2 = (
    T_w_to_c2
    @ T_c1_to_w
)

同理：

T_c2_to_c1 = (
    T_w_to_c1
    @ T_c2_to_w
)

九、为两台相机生成同一个世界平面的深度

设置世界平面：

$$Z_w=4$$

平面上的任何点都满足：

$$P_w= \begin{bmatrix} X_w\\ Y_w\\ 4 \end{bmatrix}$$

每台相机的射线：

$$r_c=K^{-1} \begin{bmatrix} u\\ v\\ 1 \end{bmatrix}$$

变到世界坐标后的方向：

$$r_w=R_{w \leftarrow c}r_c$$

相机中心：

$$C_w=t_{w \leftarrow c}$$

射线方程：

$$P_w(\lambda)=C_w+\lambda r_w$$

由于目标平面满足：

$$P_{w,z}=4$$

因此：

$$C_{w,z}+\lambda r_{w,z}=4$$

解得：

$$\lambda=\frac{4-C_{w,z}}{r_{w,z}}$$

这里的相机射线使用：

$$r_{c,z}=1$$

所以这个 $\lambda$ 正好就是相机坐标中的 z-depth。

⭐ 为什么 depth_z = numerator / denominator

⭐ 从世界射线方程推导每个像素的 z-depth

世界射线方程是：

$$p_w(\lambda)=C_w+\lambda d_w$$

其中：

• $C_w$ 是相机中心在世界坐标系中的位置。
• $d_w$ 是当前像素射线在世界坐标系中的方向。
• $\lambda$ 是沿着射线前进的距离参数。

写出 z 分量：

$$p_{w,z}(\lambda)=C_{w,z}+\lambda d_{w,z}$$

要求交点位于世界平面：

$$p_{w,z}(\lambda)=plane\_z\_w$$

代入：

$$C_{w,z}+\lambda d_{w,z}=plane\_z\_w$$

移项：

$$\lambda d_{w,z}=plane\_z\_w-C_{w,z}$$

所以：

$$\lambda=\frac{plane\_z\_w-C_{w,z}}{d_{w,z}}$$

代码中的对应关系是：

numerator = plane_z_w - camera_center_w[2]
denominator = ray_directions_w[..., 2]

也就是：

$$numerator=plane\_z\_w-C_{w,z}$$$$denominator=d_{w,z}$$

最终：

depth_z = numerator / denominator

对应数学式：

$$depth\_z=\lambda=\frac{plane\_z\_w-C_{w,z}}{d_{w,z}}$$

为什么这个 $\lambda$ 可以直接当作相机 z-depth？关键在于前面生成的相机射线不是单位射线，而是满足：

$$r_{c,z}=1$$

如果相机坐标系中的点为：

$$P_c=\lambda r_c$$

那么它的 z 分量就是：

$$Z_c=\lambda r_{c,z}=\lambda$$

所以求出的 $\lambda$ 就是每个像素的相机 z-depth。

⭐ plane_z_w 是什么？为什么这里是固定的？

plane_z_w 表示世界坐标系中目标平面的 z 坐标。

在这个实验里设置：

plane_z_w = 4.0

意思是：我们人为构造了一个位于世界坐标系中的平面，平面上所有点都满足：

$$Z_w=4$$

也就是说，任意落在这个平面上的三维点都可以写成：

$$P_w= \begin{bmatrix} X_w\\ Y_w\\ 4 \end{bmatrix}$$

其中 $X_w$ 和 $Y_w$ 可以变化，但 $Z_w$ 固定为 4.0。

这里之所以把 plane_z_w 固定，是因为阶段三的目标不是模拟复杂真实场景，而是先验证一件核心事情：

两台相机各自生成的 point map，能不能被正确变换到同一个世界坐标系里。

如果场景是一个已知平面，那么检查会非常直接：

points_w[..., 2] should be close to plane_z_w

也就是：

$$P_{w,z}\approx plane\_z\_w$$

如果变换方向、旋转矩阵、平移向量、行列向量约定都正确，那么相机 1 和相机 2 生成的世界点都应该落在同一个平面：

$$Z_w=4$$

所以固定 plane_z_w 是一种教学和单元测试设计：它让真值几何非常简单，方便判断代码有没有写错。

真实场景中通常不会有一个全局固定的 plane_z_w。真实物体可能有不同深度、曲面、遮挡和不连续边界。此时每个像素的深度来自传感器、渲染器、SDF、NeRF、MVS 或其他重建算法，而不是由一个固定平面公式直接算出来。

所以要区分：

本实验：人为设定 Z_w = 4 的平面，用来验证多视角坐标变换。
真实重建：每个像素可能对应不同深度，不能假设 plane_z_w 固定。

核心代码：

def render_world_plane_z_depth(
    K,
    height,
    width,
    T_c_to_w,
    plane_z_w=4.0,
):
    rays_c = make_camera_rays(
        K,
        height,
        width,
    )

    R_c_to_w = T_c_to_w[:3, :3]
    camera_center_w = T_c_to_w[:3, 3]

    ray_directions_w = (
        rays_c @ R_c_to_w.T
    )

    denominator = ray_directions_w[..., 2]
    numerator = (
        plane_z_w
        - camera_center_w[2]
    )

    valid = denominator > 1e-12

    depth_z = np.full(
        (height, width),
        np.nan,
    )

    depth_z[valid] = (
        numerator
        / denominator[valid]
    )

    valid &= depth_z > 0
    depth_z[~valid] = np.nan

    points_c = z_depth_to_points(
        depth_z,
        K,
    )

    points_w = transform_points(
        T_c_to_w,
        points_c,
    )

    return (
        depth_z,
        points_c,
        points_w,
        valid,
    )

十、分别生成两个 Camera-space Point Map

相机 1：

(
    depth_z_1,
    points_c1,
    points_w_1,
    valid_1,
) = render_world_plane_z_depth(
    K,
    height,
    width,
    T_c1_to_w,
    plane_z_w=4.0,
)

相机 2：

(
    depth_z_2,
    points_c2,
    points_w_2,
    valid_2,
) = render_world_plane_z_depth(
    K,
    height,
    width,
    T_c2_to_w,
    plane_z_w=4.0,
)

其中：

points_c1[v, u]

表达在相机 1 坐标系中。

points_c2[v, u]

表达在相机 2 坐标系中。

这两个 point map 不能直接拼接，因为它们属于不同坐标系。

错误做法：

merged = np.concatenate([
    points_c1,
    points_c2,
])

正确做法是先转到共同世界坐标系。

十一、转为 World-space Point Map

执行：

$$P_{w,1}=T_{w \leftarrow c_1}P_{c_1}$$ $$P_{w,2}=T_{w \leftarrow c_2}P_{c_2}$$

代码：

points_w_1 = transform_points(
    T_c1_to_w,
    points_c1,
)

points_w_2 = transform_points(
    T_c2_to_w,
    points_c2,
)

现在：

points_w_1
points_w_2

都位于同一个世界坐标系中，可以合并。

十二、检查两个点云是否落在同一平面

由于真值世界平面是：

$$Z_w=4$$

所以检查：

plane_error_1 = np.abs(
    points_w_1[..., 2][valid_1]
    - 4.0
)

plane_error_2 = np.abs(
    points_w_2[..., 2][valid_2]
    - 4.0
)

断言：

assert plane_error_1.max() < 1e-10
assert plane_error_2.max() < 1e-10

实际结果：

camera 1 max error: 0.0
camera 2 max error: 4.44e-16

这表明两台相机生成的点都被正确变换到了世界平面：

$$Z_w=4$$

十三、再做一次坐标往返检查

对于相机 2：

$$P_{w,2}=T_{w \leftarrow c_2}P_{c_2}$$

再变回相机 2：

$$P'_{c_2}=T_{c_2 \leftarrow w}P_{w,2}$$

应该满足：

$$P'_{c_2}\approx P_{c_2}$$

代码：

points_c2_recovered = transform_points(
    T_w_to_c2,
    points_w_2,
)

np.testing.assert_allclose(
    points_c2_recovered[valid_2],
    points_c2[valid_2],
    atol=1e-10,
)

这能验证：

• T_c2_to_w 的方向。
• T_w_to_c2 的逆。
• transform_points() 的行向量实现。
• 平移和旋转组合是否正确。

十四、给世界点附加 RGB

颜色最好由世界位置决定，而不是由相机编号决定。

例如：

def world_color(points_w):
    x = points_w[..., 0]
    y = points_w[..., 1]

    r = 0.5 + 0.5 * np.sin(2.0 * x)
    g = 0.5 + 0.5 * np.sin(2.0 * y + 1.0)
    b = 0.5 + 0.5 * np.sin(
        1.4 * (x + y) + 2.0
    )

    rgb = np.stack(
        [r, g, b],
        axis=-1,
    )

    return np.clip(
        rgb,
        0.0,
        1.0,
    )

然后：

rgb_1 = world_color(points_w_1)
rgb_2 = world_color(points_w_2)

这样同一个世界位置在两台相机中具有相同颜色。

相比“相机 1 全红、相机 2 全蓝”，这种方式更接近真实 RGB 采样。调试时也可以额外做一张 camera-ID 着色图，让相机 1 和相机 2 使用不同颜色，这样更容易看出是否产生双层表面。

十五、合并 [P_w, RGB]

先展平：

points_1_flat = points_w_1.reshape(-1, 3)
rgb_1_flat = rgb_1.reshape(-1, 3)

points_2_flat = points_w_2.reshape(-1, 3)
rgb_2_flat = rgb_2.reshape(-1, 3)

过滤无效点：

valid_flat_1 = (
    np.isfinite(points_1_flat).all(axis=1)
    & np.isfinite(rgb_1_flat).all(axis=1)
)

valid_flat_2 = (
    np.isfinite(points_2_flat).all(axis=1)
    & np.isfinite(rgb_2_flat).all(axis=1)
)

合并世界坐标：

merged_points_w = np.concatenate(
    [
        points_1_flat[valid_flat_1],
        points_2_flat[valid_flat_2],
    ],
    axis=0,
)

合并颜色：

merged_rgb = np.concatenate(
    [
        rgb_1_flat[valid_flat_1],
        rgb_2_flat[valid_flat_2],
    ],
    axis=0,
)

得到 XYZRGB：

merged_xyzrgb = np.concatenate(
    [
        merged_points_w,
        merged_rgb,
    ],
    axis=-1,
)

形状为：

merged_xyzrgb.shape == (N, 6)

每一行保存：

X_w, Y_w, Z_w, R, G, B

保存数组：

np.save(
    "stage3_merged_xyzrgb.npy",
    merged_xyzrgb,
)

十六、为什么两台相机的点不会逐点完全相同

虽然两台相机观察的是同一个平面，但：

points_w_1[v, u]

和：

points_w_2[v, u]

通常不是同一个世界点。

原因是两台相机位置不同、朝向不同，相同像素 (u, v) 对应不同世界射线。所以不能直接检查：

points_w_1 == points_w_2

这是错误的比较方式。

正确检查包括：

• 两者是否都落在同一真实表面。
• 重叠区域中最近邻距离是否较小。
• 跨视图重投影是否一致。
• 已知对应点是否在世界坐标中重合。

本实验做了平面残差检查和最近邻检查。

十七、如何理解最近邻误差

脚本中对点云进行了降采样：

stride = 4

然后计算一个点云到另一个点云的最近邻距离。结果大约：

median: 0.0215
p95:    0.0313
max:    0.0395

这不代表平面偏差有 2 厘米。

两个点云都严格位于：

$$Z_w=4$$

但它们在平面上的采样网格不同，而且又进行了降采样，因此某个点通常找不到完全相同的采样点，只能找到附近的点。

真正的平面方向误差是：

0 and 4.44e-16

几乎为零。

十八、出现双层表面时怎样判断原因

1. 把 T_c2_to_w 用成了 T_w_to_c2

正确：

points_w_2 = transform_points(
    T_c2_to_w,
    points_c2,
)

错误：

points_w_2 = transform_points(
    T_w_to_c2,
    points_c2,
)

现象：

• 相机 2 点云出现在错误位置。
• 平面可能倾斜。
• 两层表面之间有明显平移。
• 相机中心位置也可能错误。

2. 复合顺序写反

正确：

T_c1_to_c2 = (
    T_w_to_c2
    @ T_c1_to_w
)

错误：

T_c1_to_c2 = (
    T_c1_to_w
    @ T_w_to_c2
)

矩阵乘法一般不可交换：

$$AB \neq BA$$

3. 行向量和列向量混用

正确的行向量 NumPy 写法：

points_dst = points_src @ R.T + t

如果错误写成：

points_dst = points_src @ R + t

会使用反向或错误的旋转。

4. 相机位置和外参平移混淆

对于：

$$T_{w \leftarrow c}$$

平移部分：

$$t_{w \leftarrow c}$$

是相机中心的世界坐标。

但是对于：

$$T_{c \leftarrow w}$$

其中的平移：

$$t_{c \leftarrow w}$$

通常不是相机中心。相机中心应由：

$$C_w=-R_{c \leftarrow w}^{T}t_{c \leftarrow w}$$

计算。

5. 深度单位不同

例如：

• 相机 1 深度单位为米。
• 相机 2 深度单位为毫米。
• 相机平移单位为米。

会导致点云尺度相差 1000 倍。单相机 round-trip 仍可能通过，但多视图合并会完全失败。

必须保证：

• depth
• translation
• baseline
• world coordinates

使用同一长度单位。

6. resize 后没有更新内参

原图宽高：

640 x 480

缩放成：

320 x 240

如果缩放因子为 0.5，则：

$$f'_x=0.5f_x$$ $$f'_y=0.5f_y$$ $$c'_x=0.5c_x$$ $$c'_y=0.5c_y$$

否则两个点云可能表现为：

• 尺寸不同。
• 平面边缘错开。
• 点云呈不同程度的展开。
• 中心附近看似对齐，边缘逐渐分离。

十九、这一阶段仍然有什么局限

单个无限平面虽然适合初始测试，但几何约束比较弱。某些错误会让两个点云仍然近似落在同一平面，只是在平面内部发生滑动。

平面测试通过后，建议追加一个非共面场景：

• 背景平面 Z=4
• 前景平面 Z=3
• 一个立方体
• 或三个不同深度的小物体

非共面场景能更容易暴露：

• 旋转方向错误。
• 平移方向错误。
• 深度尺度错误。
• 相机间相对位姿错误。

二十、本阶段核心结论

这一阶段最核心的关系是：

$$P_{w,i}=T_{w \leftarrow c_i}P_{c_i}$$

只有当所有局部 point map 都被正确转换到同一个坐标系后，才可以执行：

$$P_w=\bigcup_i P_{w,i}$$

下一阶段最有价值的验证，是把相机 1 的世界点变换到相机 2，并投影到图像 2，完成真正的跨视角重投影与深度一致性测试。