阶段一：单相机、合成数据

下面把“阶段一”拆成一个可直接运行、可验证、可观察错误现象的小实验。代码已经运行过，所有数值检查均通过。

本阶段产物：

• pointmap_stage1.py
• constant_z_depth_plane.png
• constant_euclidean_depth_sphere.png

运行方式：

pip install numpy matplotlib
python pointmap_stage1.py

1. 首先固定坐标系约定

这里使用常见的 OpenCV 相机坐标系：

+x: 图像右方
+y: 图像下方
+z: 相机前方

因此，对于图像中的像素：

• 右侧像素对应射线的 $x>0$。
• 下方像素对应射线的 $y>0$。
• 所有可见点通常满足 $z>0$。

图像数组仍然按照：

image[v, u]

访问，其中：

• $u$：横坐标，对应 width。
• $v$：纵坐标，对应 height。

这是一个非常常见的混淆点：

points[v, u]

而不是：

points[u, v]

2. 构造分辨率和相机内参

设置：

width = 320
height = 240

fx = 300.0
fy = 300.0
cx = width / 2.0
cy = height / 2.0

相机内参：

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

对应代码：

K = np.array(
    [
        [fx, 0.0, cx],
        [0.0, fy, cy],
        [0.0, 0.0, 1.0],
    ],
    dtype=np.float64,
)

此时：

$$K= \begin{bmatrix} 300 & 0 & 160 \\ 0 & 300 & 120 \\ 0 & 0 & 1 \end{bmatrix}$$

我们故意取：

$$c_x=160,\quad c_y=120$$

这样整数像素：

$$u=160,\quad v=120$$

正好位于主光轴上，它的射线严格为：

$$(0,0,1)$$

一个细节：偶数尺寸图像的几何中心

对于 320 x 240 这样的偶数尺寸，图像的几何中心实际上位于：

$$(159.5,\ 119.5)$$

也就是四个像素之间。

真实相机标定可能给出：

$$c_x \approx 159.5,\quad c_y \approx 119.5$$

那么不会有任何一个整数像素的射线严格等于 $(0,0,1)$。这里设置成 $(160,120)$，只是为了让第一个教学实验更直观。

实际工程中应始终使用真实标定得到的 $K$。

3. 任务一：生成 Camera Ray Map

对于像素：

$$\tilde{p} = \begin{bmatrix} u \\ v \\ 1 \end{bmatrix}$$

对应的相机射线为：

$$r=K^{-1}\tilde{p}$$

展开后：

$$r= \begin{bmatrix} \frac{u-c_x}{f_x} \\ \frac{v-c_y}{f_y} \\ 1 \end{bmatrix}$$

注意，这里得到的射线通常不是单位向量，而是让：

$$r_z=1$$

它特别适合与 z-depth 相乘。

核心实现：

def make_camera_rays(
    K: np.ndarray,
    height: int,
    width: int,
    *,
    normalize: bool = False,
) -> np.ndarray:
    K = np.asarray(K, dtype=np.float64)
    K_inv = np.linalg.inv(K)

    u, v = np.meshgrid(
        np.arange(width, dtype=np.float64),
        np.arange(height, dtype=np.float64),
        indexing="xy",
    )

    pixels_h = np.stack(
        [u, v, np.ones_like(u)],
        axis=-1,
    )

    rays = pixels_h @ K_inv.T

    if normalize:
        lengths = np.linalg.norm(
            rays,
            axis=-1,
            keepdims=True,
        )
        rays = rays / np.clip(lengths, 1e-12, None)

    return rays

输出形状：

rays.shape == (240, 320, 3)

即：

$$R\in \mathbb{R}^{240\times 320\times 3}$$

其中：

rays[v, u]

就是像素 $(u,v)$ 对应的相机射线。

⭐ make_camera_rays() 逐行解释

⭐ make_camera_rays() 逐行解释

这段函数的作用是：根据相机内参矩阵 $K$，为图像中的每个像素生成一条从相机光心出发的射线方向。

输出形状是：

(height, width, 3)

每个像素对应一个三维向量：

[x, y, 1]

或者归一化后的单位向量。

1. 函数定义

def make_camera_rays(
    K: np.ndarray,
    height: int,
    width: int,
    *,
    normalize: bool = False,
) -> np.ndarray:

等价于较紧凑的写法：

def make_camera_rays(K, height, width, *, normalize=False):

这里只是增加了类型标注。

K: np.ndarray 表示参数 $K$ 预期是 NumPy 数组。这是类型提示，不会强制检查类型。例如下面仍然可以调用：

make_camera_rays(
    [[500, 0, 160],
     [0, 500, 120],
     [0, 0, 1]],
    240,
    320,
)

因为函数内部会把它转换成数组。

height: int 表示图像高度应该是整数，例如：

height = 240

width: int 表示图像宽度应该是整数，例如：

width = 320

单独的 *：

*,
normalize: bool = False,

表示 * 后面的参数必须通过参数名传递。因此可以这样调用：

make_camera_rays(K, 240, 320, normalize=True)

但不能这样调用：

make_camera_rays(K, 240, 320, True)

这样设计可以让代码更容易读，因为看到 normalize=True 就能立刻知道 True 的含义。

normalize: bool = False 表示：

• normalize 应当是布尔值。
• 默认值是 False。

如果调用时不传：

rays = make_camera_rays(K, 240, 320)

等价于：

rays = make_camera_rays(
    K,
    240,
    320,
    normalize=False,
)

如果设置为：

normalize=True

则每条射线会被归一化为长度 1 的单位向量。

-> np.ndarray 表示函数预期返回一个 NumPy 数组。同样，这只是类型提示，不是运行时强制约束。

2. 将 K 转换为 NumPy 数组

K = np.asarray(K, dtype=np.float64)

np.asarray() 将输入转换为 NumPy 数组。例如输入是列表：

K = [
    [500, 0, 160],
    [0, 500, 120],
    [0, 0, 1],
]

执行后会得到：

array([
    [500.,   0., 160.],
    [  0., 500., 120.],
    [  0.,   0.,   1.]
])

dtype=np.float64 指定使用 64 位浮点数。这是必要的，因为后面需要求逆矩阵：

np.linalg.inv(K)

如果全部使用整数，不适合进行逆矩阵和除法运算。

3. 计算内参矩阵的逆

K_inv = np.linalg.inv(K)

相机内参通常写为：

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

像素坐标与相机射线之间满足：

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

所以反过来：

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

因此需要计算 K_inv，也就是 $K^{-1}$。

4. 创建所有像素坐标

u, v = np.meshgrid(
    np.arange(width, dtype=np.float64),
    np.arange(height, dtype=np.float64),
    indexing="xy",
)

这一部分生成每个像素的横纵坐标。

np.arange(width, dtype=np.float64)

生成横坐标：

[0, 1, 2, ..., width - 1]

例如 width = 4 得到：

[0., 1., 2., 3.]

对应像素横坐标 $u$。

np.arange(height, dtype=np.float64)

生成纵坐标：

[0, 1, 2, ..., height - 1]

例如 height = 3 得到：

[0., 1., 2.]

对应像素纵坐标 $v$。

假设：

width = 4
height = 3

执行：

u, v = np.meshgrid(
    np.arange(4),
    np.arange(3),
    indexing="xy",
)

得到：

u =
[[0, 1, 2, 3],
 [0, 1, 2, 3],
 [0, 1, 2, 3]]

v =
[[0, 0, 0, 0],
 [1, 1, 1, 1],
 [2, 2, 2, 2]]

形状都是：

(height, width)

即：

(3, 4)

其中 u[v_index, u_index] 保存横坐标，v 保存纵坐标。

indexing="xy" 表示按照图像或笛卡尔坐标习惯生成：

• 第一个输入是横坐标 $x/u$。
• 第二个输入是纵坐标 $y/v$。

对于图像，结果形状是：

(height, width)

5. 构造齐次像素坐标

pixels_h = np.stack(
    [u, v, np.ones_like(u)],
    axis=-1,
)

每个像素原本有两个坐标：

$$(u,v)$$

现在补一个 1，构造齐次坐标：

$$(u,v,1)$$

np.ones_like(u) 生成一个与 u 形状相同、数据类型相同、元素全部为 1 的数组。

例如：

u.shape == (240, 320)

那么：

np.ones_like(u).shape == (240, 320)

内容类似：

[[1., 1., 1., ...],
 [1., 1., 1., ...],
 ...]

np.stack([u, v, ones], axis=-1) 把三个形状为：

(height, width)

的数组沿最后一个维度拼起来，得到：

pixels_h.shape == (height, width, 3)

每个像素的位置保存：

pixels_h[row, col] = [u, v, 1]

例如第 20 行、第 30 列：

pixels_h[20, 30]

结果是：

[30., 20., 1.]

注意图像数组索引是：

image[v, u]

也就是：

image[行, 列]

但保存的坐标顺序是：

[u, v, 1]

axis=-1 表示最后一个维度。例如：

(height, width)

增加一个最后维度后变成：

(height, width, 3)

如果写成 axis=0，则会变成：

(3, height, width)

这不符合当前代码采用的 $H\times W\times 3$ 格式。

6. 像素坐标转换为相机射线

rays = pixels_h @ K_inv.T

@ 是 Python 的矩阵乘法运算符。例如：

A @ B

通常等价于：

np.matmul(A, B)

公式通常写成列向量形式：

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

但代码中的像素向量是行向量：

[u, v, 1]

所以需要写成：

$$[u,v,1]K^{-T}$$

也就是：

pixels_h @ K_inv.T

这里：

pixels_h.shape == (height, width, 3)
K_inv.T.shape == (3, 3)

NumPy 会让每个长度为 3 的像素向量乘以该矩阵，最终得到：

rays.shape == (height, width, 3)

也可以使用 einsum：

rays = np.einsum(
    "ij,hwj->hwi",
    K_inv,
    pixels_h,
)

二者含义相同。

7. 射线的具体公式

对于标准内参：

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

计算结果等价于：

$$r(u,v)= \begin{bmatrix} \frac{u-c_x}{f_x} \\ \frac{v-c_y}{f_y} \\ 1 \end{bmatrix}$$

也就是：

rays[..., 0] = (u - cx) / fx
rays[..., 1] = (v - cy) / fy
rays[..., 2] = 1

在 OpenCV 相机坐标系中：

• $x$ 向右。
• $y$ 向下。
• $z$ 向前。

因此：

• 图像右侧像素通常满足 $x>0$。
• 图像左侧像素通常满足 $x<0$。
• 图像下方像素通常满足 $y>0$。
• 图像上方像素通常满足 $y<0$。

主点附近：

$$u\approx c_x,\quad v\approx c_y$$

射线接近：

[0, 0, 1]

8. 判断是否进行归一化

if normalize:

意思是：

if normalize == True:

但 Python 中更推荐直接写：

if normalize:

当调用：

make_camera_rays(..., normalize=False)

这部分不会执行。

当调用：

make_camera_rays(..., normalize=True)

才会执行归一化。

9. 计算每条射线的长度

lengths = np.linalg.norm(
    rays,
    axis=-1,
    keepdims=True,
)

对于每条三维射线：

[x, y, z]

长度是：

$$\sqrt{x^2+y^2+z^2}$$

np.linalg.norm(rays, axis=-1) 计算向量范数，也就是欧氏长度。

假设：

rays.shape == (240, 320, 3)

沿最后一个维度计算长度。axis=-1 表示对最后一个维度：

[x, y, z]

进行计算，而不是把整张图像的所有数一起计算。

如果不写 keepdims=True，结果形状是：

(height, width)

写了以后结果是：

(height, width, 1)

这样方便后面进行广播除法：

rays / lengths

形状对应为：

(height, width, 3) / (height, width, 1)

最后一个长度值会自动作用于同一像素的三个分量。

10. 防止除以零并进行归一化

rays = rays / np.clip(lengths, 1e-12, None)

np.clip(lengths, 1e-12, None) 表示把 lengths 限制在：

[1e-12, +infinity)

范围内。

也就是说：

• 小于 1e-12 的值改成 1e-12。
• 较大的值保持不变。
• None 表示没有上限。

这样可以防止：

rays / 0

产生无穷大或 NaN。

虽然正常相机射线的第三个分量通常为 1，因此长度不会是零，但这种写法更加稳健。

单位化：

rays = rays / lengths

等价于：

$$\hat{r}=\frac{r}{\|r\|}$$

归一化后，每条射线的长度约等于 1：

np.linalg.norm(rays[v, u]) ≈ 1

11. 返回结果

return rays

返回形状：

(height, width, 3)

的数组。

访问某个像素的射线：

ray = rays[v, u]

注意数组索引是：

[v, u]

而不是：

[u, v]

4. 检查 Ray Map

中心像素：

rays[120, 160]

输出：

[0. 0. 1.]

因为：

$$u=c_x,\quad v=c_y$$

所以：

$$r= \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix}$$

右侧边缘：

rays[120, 319]

输出：

[0.53 0.   1.  ]

其中：

$$x=\frac{319-160}{300}=0.53>0$$

下侧边缘：

rays[239, 160]

输出：

[0.         0.39666667 1.        ]

其中：

$$y=\frac{239-120}{300}\approx 0.3967>0$$

对应断言：

np.testing.assert_allclose(
    rays[120, 160],
    [0.0, 0.0, 1.0],
    atol=1e-12,
)

assert rays[120, 319, 0] > 0
assert rays[239, 160, 1] > 0

这些检查可以快速发现：

• $u,v$ 写反
• width、height 写反
• $y$ 轴方向弄反
• 内参主点使用错误
• 矩阵转置错误

5. 为什么需要两类 Ray Map

需要保留两种形式。

z 为 1 的射线

rays_z1 = make_camera_rays(
    K,
    height,
    width,
    normalize=False,
)

满足：

$$r_z=1$$

适合 z-depth：

$$P_c=Zr$$

单位射线

unit_rays = make_camera_rays(
    K,
    height,
    width,
    normalize=True,
)

满足：

$$\|r\|_2=1$$

适合 Euclidean depth：

$$P_c=D_e r_{unit}$$

检查单位长度：

np.testing.assert_allclose(
    np.linalg.norm(unit_rays, axis=-1),
    1.0,
    atol=1e-12,
)

这就是为什么不应模糊地只实现一个 depth_to_points()：不同深度定义需要不同的射线处理。

6. 任务二：z-depth 转 Camera-space Point Map

z-depth 表示三维点的 $Z$ 坐标：

$$D_z(u,v)=Z$$

因此：

$$P_c(u,v)=D_z(u,v)K^{-1}\tilde{p}$$

展开为：

$$X=\frac{u-c_x}{f_x}Z$$ $$Y=\frac{v-c_y}{f_y}Z$$ $$Z=D_z$$

实现：

def z_depth_to_points(
    depth_z: np.ndarray,
    K: np.ndarray,
) -> np.ndarray:
    height, width = depth_z.shape

    rays_z1 = make_camera_rays(
        K,
        height,
        width,
        normalize=False,
    )

    points = rays_z1 * depth_z[..., None]
    return points

这里：

depth_z.shape

是：

(H, W)

而：

depth_z[..., None].shape

是：

(H, W, 1)

它会通过广播与：

(H, W, 3)

的 ray map 相乘。

最终：

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

对一个像素手算

例如：

$$u=310,\quad v=120,\quad Z=2$$

则：

$$X=\frac{310-160}{300}\times 2=1$$$$Y=\frac{120-120}{300}\times 2=0$$$$Z=2$$

所以：

$$P_c=(1,0,2)$$

注意它的欧氏距离不是 2：

$$\|P_c\|_2 = \sqrt{1^2+0^2+2^2} = \sqrt{5} \approx 2.236$$

这说明：z-depth 是光轴方向深度，不是相机到点的直线距离。

7. Euclidean depth 转 Camera-space Point Map

Euclidean depth 定义为：

$$D_e(u,v)=\|P_c(u,v)\|_2$$

因此不能直接使用 z=1 的射线相乘，因为：

$$\left\|K^{-1}\tilde{p}\right\|_2$$

通常不等于 1。

必须先将射线单位化：

$$\hat{r} = \frac{K^{-1}\tilde{p}}{\left\|K^{-1}\tilde{p}\right\|_2}$$

然后：

$$P_c=D_e\hat{r}$$

实现：

def euclidean_depth_to_points(
    depth_e: np.ndarray,
    K: np.ndarray,
) -> np.ndarray:
    height, width = depth_e.shape

    unit_rays = make_camera_rays(
        K,
        height,
        width,
        normalize=True,
    )

    points = unit_rays * depth_e[..., None]
    return points

此时一定满足：

$$\|P_c(u,v)\|_2=D_e(u,v)$$

8. 任务三：构造常数深度

首先构造常数 z-depth：

depth_z = np.full(
    (height, width),
    2.0,
    dtype=np.float64,
)

即：

$$D_z(u,v)=2$$

转换：

points_from_z = z_depth_to_points(
    depth_z,
    K,
)

检查所有点的 $Z$：

np.testing.assert_allclose(
    points_from_z[..., 2],
    2.0,
    atol=1e-12,
)

结果：

Z min/max: 2.0 2.0

所以所有点都位于：

$$Z=2$$

这是一张与图像平面平行的平面。

但它们距离相机并不都等于 2

计算：

distance = np.linalg.norm(
    points_from_z,
    axis=-1,
)

实际输出：

Euclidean distance min/max:
2.0 2.4037008503093262

中心点距离为 2，但越靠近图像边缘，点到相机中心的直线距离越大。

因此常数 z-depth 描述的是：

一张平面

而不是球面。

9. 构造常数 Euclidean depth

depth_e = np.full(
    (height, width),
    2.0,
    dtype=np.float64,
)

转换：

points_from_e = euclidean_depth_to_points(
    depth_e,
    K,
)

检查欧氏距离：

euclidean_norms = np.linalg.norm(
    points_from_e,
    axis=-1,
)

np.testing.assert_allclose(
    euclidean_norms,
    2.0,
    atol=1e-12,
)

实际输出：

Euclidean distance min/max:
1.9999999999999996
2.0000000000000004

浮点误差约为：

$$4.4\times 10^{-16}$$

也就是说所有点都满足：

$$X^2+Y^2+Z^2=2^2$$

它们位于以相机中心为球心、半径为 2 的球面上。

但由于相机只看到前方有限视场，因此得到的不是完整球体，而是一个球面片。

它的 Z 不再恒定

实际结果：

Z min/max:
1.6641005886756874 2.0

中心射线方向为：

$$(0,0,1)$$

所以中心点：

$$P_c=(0,0,2)$$

其 $Z=2$。

边缘射线发生倾斜，为了保持欧氏距离仍然等于 2，它的 $Z$ 必须小于 2。

10. 两个实验的核心区别

常数 z-depth

$$D_z=2$$

得到：

$$Z=2$$

因此形成平面：

$$Z=2$$

但：

$$\|P_c\|_2$$

会随像素位置改变。

常数 Euclidean depth

$$D_e=2$$

得到：

$$\|P_c\|_2=2$$

因此形成球面：

$$X^2+Y^2+Z^2=4$$

但：

$$Z$$

会随像素位置改变。

可以记成：

深度类型	恒定时形成什么	保持不变的量
z-depth	平面	$Z$
Euclidean depth	球面片	$\sqrt{X^2+Y^2+Z^2}$

11. 加一个投影往返检查

阶段一还应该实现一个简单投影函数：

$$u=f_x\frac{X}{Z}+c_x$$ $$v=f_y\frac{Y}{Z}+c_y$$

代码：

def project_camera_points(points_c, K):
    x = points_c[..., 0]
    y = points_c[..., 1]
    z = points_c[..., 2]

    valid = (
        np.isfinite(points_c).all(axis=-1)
        & (z > 0)
    )

    u = K[0, 0] * x / z + K[0, 2]
    v = K[1, 1] * y / z + K[1, 2]

    uv = np.stack([u, v], axis=-1)
    return uv, z, valid

执行：

pixel + z-depth
    ↓
camera-space point
    ↓
projection
    ↓
pixel + z-depth

检查：

max_pixel_error < 1e-10
max_z_error < 1e-10

本次运行结果：

max pixel error:
5.684341886080802e-14

max z-depth error:
0.0

这能验证：

• 反投影公式正确
• 投影公式正确
• $u,v$ 没有颠倒
• $K$ 的使用基本正确

但它不能验证真实尺度，因为同相机投影对于整体尺度不敏感。跨视角验证要留到下一阶段。

12. 可视化时为何需要降采样

完整 point-map 有：

$$320\times 240=76800$$

个点。

全部画出来没有必要，因此代码中每隔 8 个像素采样一次：

points = points[::8, ::8]

然后画三维散点。

z-depth 图应观察到：

• 所有点的 $Z$ 相同
• 点排列成矩形平面
• 越靠边，$X,Y$ 绝对值越大

Euclidean depth 图应观察到：

• 点形成向相机方向凸出的球面片
• 所有点到原点距离相同
• 中心点 $Z$ 最大
• 越靠边，$Z$ 越小

13. 建议逐个做的错误实验

在正确代码运行后，主动制造这些错误。

错误一：Euclidean depth 不归一化射线

错误写法：

points = depth_e[..., None] * rays_z1

观察结果：

• 它不会形成半径为 2 的球面
• 距离相机越靠边越大
• 实际上等价于把 Euclidean depth 错当成 z-depth

检查会失败：

np.linalg.norm(points, axis=-1)

不再恒定。

错误二：交换 u 和 v

错误写法：

pixels_h = np.stack(
    [v, u, np.ones_like(u)],
    axis=-1,
)

可能导致：

• $x,y$ 方向错位
• 非方形图像中形状严重扭曲
• 中心射线可能不再正确
• 重投影检查失败

错误三：把 y 轴改成向上

例如错误地写成：

y = -(v - cy) / fy

这不是绝对错误，而是变成了另一套坐标约定。

问题在于：假如后续仍按 OpenCV 坐标系解释，就会造成点云上下翻转。

所以重要的不是只能采用一种坐标系，而是：

生成、变换、投影和可视化必须始终使用同一套约定。

错误四：使用错误的主点

例如：

cx = 0
cy = 0

那么左上角像素会被当成光轴中心，整个点云会向一侧倾斜。

错误五：错误归一化 z-depth 射线

假如 z-depth 转点时使用单位射线：

points = depth_z[..., None] * unit_rays

那么：

$$\|P_c\|=D_z$$

而不是：

$$Z=D_z$$

得到的是球面片，不是 $Z=2$ 的平面。

14. 这一阶段真正应该掌握的内容

完成实验后，应能不看代码回答以下问题：

• 为什么 ray[..., 2] = 1 的射线适合 z-depth？
• 为什么 Euclidean depth 必须乘单位射线？
• 为什么常数 z-depth 会形成平面？
• 为什么常数 Euclidean depth 会形成球面片？
• 为什么图像数组索引是 [v, u]？
• 为什么 OpenCV 图像下方对应相机坐标 $y>0$？
• 为什么偶数尺寸图像可能没有精确的中心像素？
• 为什么同相机重投影正确不能证明深度尺度正确？
• point map 为什么是 $H\times W\times 3$，而普通点云通常是 $N\times 3$？
• 未归一化 ray map 和单位 ray map 分别适合什么深度定义？

这一阶段的核心不是“把深度变成点云”，而是建立下面这条关系：

$$\text{pixel} \xrightarrow{K^{-1}} \text{camera ray} \xrightarrow{\text{depth definition}} \text{camera-space 3D point}$$

其中最关键的一句是：

$$\boxed{ \text{z-depth 乘 } z=1 \text{ 射线；Euclidean depth 乘单位射线} }$$