自写实验：盒子深度与双相机射线

这篇 note 记录一段自己写的 ray map、盒子深度渲染和双相机实验代码。它和阶段三的关系比较近：目标是从一个世界坐标系中的 box 出发，为相机生成 depth，再进一步转成 camera-space point map。

代码想做什么

整体意图可以概括为：

构造相机内参 K
-> 为每个像素生成 camera ray
-> 构造两个相机位姿 T_c1_to_w / T_c2_to_w
-> 用 ray-box intersection 渲染 box 的 depth
-> 可视化 depth
-> 准备把 depth 转成 camera point map

其中最核心的两个函数是：

• calculate_rays()：根据相机内参生成 ray map。
• render_box_depth()：用 ray 和 axis-aligned box 求交，生成 depth map。

原始代码

import numpy as np

width = 320
height = 240

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

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

def calculate_rays(
    K,width,height,normalize:bool=False
)-> np.ndarray:
    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 = np.stack(
        [u,v,np.ones_like(u)],
        axis=-1
    )

    res = pixels @ K_inv.T

    if normalize:
        norm_ = np.linalg.norm(
            res,axis=-1,keepdims=True
        )
        res = res / np.clip(norm_,1e-10,None)

    return res

#generated by codex(part1)
#create a box-shape object
def render_box_depth(
    K, width, height, T_c_to_w,
    box_min_w,
    box_max_w,
):
    T_c_to_w = np.asarray(T_c_to_w, dtype=np.float64)
    R_c_to_w = T_c_to_w[:3, :3]
    origin_w = T_c_to_w[:3, 3]

    rays_c = calculate_rays(K, width, height, normalize=False)
    rays_w = rays_c @ R_c_to_w.T

    box_min_w = np.asarray(box_min_w, dtype=np.float64)
    box_max_w = np.asarray(box_max_w, dtype=np.float64)

    safe_rays = np.where(
        np.abs(rays_w) < 1e-10,
        np.copysign(1e-10, rays_w + 1e-20),
        rays_w,
    )

    t1 = (box_min_w - origin_w) / safe_rays
    t2 = (box_max_w - origin_w) / safe_rays

    t_near = np.max(np.minimum(t1, t2), axis=-1)
    t_far = np.min(np.maximum(t1, t2), axis=-1)

    valid = (t_far >= np.maximum(t_near, 0.0))

    # 相机在盒子外时取前表面；在盒子内部时取出射表面
    hit_t = np.where(t_near > 0.0, t_near, t_far)

    depth = np.full((height, width), np.nan, dtype=np.float64)
    depth[valid] = hit_t[valid]
    return depth

import matplotlib.pyplot as plt

def show_depth(depth, title="Camera depth"):
    valid = np.isfinite(depth) & (depth > 0)

    display_depth = np.ma.masked_where(~valid, depth)

    cmap = plt.colormaps["turbo"].copy()
    cmap.set_bad("black")

    plt.figure(figsize=(8, 6))
    image = plt.imshow(display_depth, cmap=cmap)
    plt.colorbar(image, label="Depth (m)")
    plt.title(title)
    plt.xlabel("u")
    plt.ylabel("v")
    plt.tight_layout()
    plt.show()

#part1 over



def rotate_y(angle):
    c = np.cos(angle)
    s = np.sin(angle)
    return np.array(
        [
        [c,0,s],
        [0,1,0],
        [-s,0,c],
        ],
        dtype=np.float64,
    )

T_c1_to_w = np.eye(4,dtype=np.float64)
R_c2_to_w = rotate_y(np.deg2rad(-30))
t_c2_to_w = np.array([0.5,0.52,0.47],dtype=np.float64)
T_c2_to_w = np.eye(4,dtype=np.float64)
T_c2_to_w[0:3,0:3] = R_c2_to_w
T_c2_to_w[0:3,3] = t_c2_to_w

# 模拟一个world 深度图,转化为 camera深度
def const_world_field_2_camera_depth(
    K,width,height,T_c_to_w,normalize:bool=False,plane_z_w=4.0
):

    depth_k = render_box_depth(
    K, width, height, T_c2_to_w,
    box_min_w=[-0.5, -0.5, 3.5],
    box_max_w=[ 0.5,  0.5, 4.5],
    )

    R_c_to_w = np.asarray(T_c_to_w,dtype=np.float64)[0:3,0:3]
    t_c_to_w = np.asarray(T_c_to_w,dtype=np.float64)[0:3,3]
    
    world_rays = calculate_rays(K,width,height,normalize) @ R_c_to_w.T
    depth_ = (depth_k - t_c_to_w[2])/world_rays[...,2] #world_rays[2]的形状会错误 
    show_depth(depth_)
    return depth_

def depths_2_points(depth_,K):
    height,width = depth.shape()
    camera_rays = calculate_rays(K,width,height)
    points = camera_rays * depth_[...,None]  #[...,None]变形后才能广播 (H, W, 3) * (H, W, 1) -> (H, W, 3)

def main():
    print(calculate_rays(K,width,height)[120,319])
    const_world_field_2_camera_depth(K,width,height,T_c2_to_w)
    return 0

if __name__ == "__main__":
    raise SystemExit(main())

这段代码里最值得保留的部分

calculate_rays() 的方向是对的：

pixels @ K_inv.T

对应的是行向量写法：

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

输出：

$$R\in\mathbb{R}^{H\times W\times 3}$$

且在 normalize=False 时，射线满足：

$$r_z=1$$

所以它适合直接乘以相机 z-depth：

$$P_c=Z_c r_c$$

render_box_depth() 使用的是标准 slab method 求 ray-box intersection。对 axis-aligned box：

$$box_{\min}\le P_w \le box_{\max}$$

射线为：

$$P_w(t)=O_w+t d_w$$

分别求每个坐标轴上的进入和离开时间，再取：

$$t_{near}=\max(t_{x,near},t_{y,near},t_{z,near})$$ $$t_{far}=\min(t_{x,far},t_{y,far},t_{z,far})$$

如果：

$$t_{far}\ge \max(t_{near},0)$$

说明射线打中了盒子。

⭐ 这里的 hit_t 是 z-depth 吗？

在这段代码里，render_box_depth() 使用的是未归一化 ray：

rays_c = calculate_rays(K, width, height, normalize=False)

因此相机射线满足：

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

世界射线是：

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

射线参数：

$$P_w(t)=O_w+t r_w$$

对应相机坐标中：

$$P_c(t)=t r_c$$

因为 $r_{c,z}=1$，所以：

$$Z_c=t r_{c,z}=t$$

也就是说，只要使用的是 normalize=False 的射线，ray-box intersection 算出来的 hit_t 就可以直接看成相机 z-depth。

如果使用单位射线 normalize=True，那么 hit_t 就是 Euclidean distance，不再等于 z-depth。

当前代码里需要修正的地方

1. const_world_field_2_camera_depth() 里用了错误的相机位姿

函数参数传进来了：

T_c_to_w

但内部调用 render_box_depth() 时写死用了：

T_c2_to_w

也就是：

depth_k = render_box_depth(
    K, width, height, T_c2_to_w,
    ...
)

这会导致函数名义上支持任意相机，实际上永远用相机 2 渲染。

应该改成：

depth_z = render_box_depth(
    K,
    width,
    height,
    T_c_to_w,
    box_min_w=[-0.5, -0.5, 3.5],
    box_max_w=[0.5, 0.5, 4.5],
)

2. depth_k 已经是 camera z-depth，不应该再套一次平面公式

这段：

world_rays = calculate_rays(K,width,height,normalize) @ R_c_to_w.T
depth_ = (depth_k - t_c_to_w[2])/world_rays[...,2]

混合了两个不同实验：

• 平面求交：$\lambda=(plane_z-C_z)/d_z$
• 盒子求交：ray-box intersection 已经直接给出 hit depth

如果 render_box_depth() 已经返回了 depth_z，就不需要再做：

(depth_k - t_c_to_w[2]) / world_rays[..., 2]

应该直接：

show_depth(depth_z)
return depth_z

3. world_rays[..., 2] 的索引写法是对的

注释里写：

# world_rays[2]的形状会错误

这句话是对的：如果写成：

world_rays[2]

取的是第 3 行像素，形状会变成：

(W, 3)

正确取所有像素的 z 分量应该是：

world_rays[..., 2]

形状是：

(H, W)

你代码里实际用的是 world_rays[...,2]，这个写法本身正确。

4. depths_2_points() 里变量名写错

原代码：

def depths_2_points(depth_,K):
    height,width = depth.shape()

这里 depth 未定义，而且 .shape 是属性，不是函数。

应改为：

height, width = depth_.shape

同时函数应该返回 points：

return points

修正版代码

下面是整理后的可运行版本：

import numpy as np
import matplotlib.pyplot as plt


width = 320
height = 240

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

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


def calculate_rays(K, width, height, normalize=False):
    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 = np.stack(
        [u, v, np.ones_like(u)],
        axis=-1,
    )

    rays = pixels @ K_inv.T

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

    return rays


def render_box_depth(
    K,
    width,
    height,
    T_c_to_w,
    box_min_w,
    box_max_w,
):
    T_c_to_w = np.asarray(T_c_to_w, dtype=np.float64)
    R_c_to_w = T_c_to_w[:3, :3]
    origin_w = T_c_to_w[:3, 3]

    rays_c = calculate_rays(
        K,
        width,
        height,
        normalize=False,
    )
    rays_w = rays_c @ R_c_to_w.T

    box_min_w = np.asarray(box_min_w, dtype=np.float64)
    box_max_w = np.asarray(box_max_w, dtype=np.float64)

    safe_rays = np.where(
        np.abs(rays_w) < 1e-10,
        np.copysign(1e-10, rays_w + 1e-20),
        rays_w,
    )

    t1 = (box_min_w - origin_w) / safe_rays
    t2 = (box_max_w - origin_w) / safe_rays

    t_near = np.max(np.minimum(t1, t2), axis=-1)
    t_far = np.min(np.maximum(t1, t2), axis=-1)

    valid = t_far >= np.maximum(t_near, 0.0)
    hit_t = np.where(t_near > 0.0, t_near, t_far)

    depth_z = np.full(
        (height, width),
        np.nan,
        dtype=np.float64,
    )
    depth_z[valid] = hit_t[valid]

    return depth_z


def depth_to_points(depth_z, K):
    height, width = depth_z.shape

    rays_c = calculate_rays(
        K,
        width,
        height,
        normalize=False,
    )

    points_c = rays_c * depth_z[..., None]
    return points_c


def show_depth(depth, title="Camera z-depth"):
    valid = np.isfinite(depth) & (depth > 0)
    display_depth = np.ma.masked_where(~valid, depth)

    cmap = plt.colormaps["turbo"].copy()
    cmap.set_bad("black")

    plt.figure(figsize=(8, 6))
    image = plt.imshow(display_depth, cmap=cmap)
    plt.colorbar(image, label="z-depth (m)")
    plt.title(title)
    plt.xlabel("u")
    plt.ylabel("v")
    plt.tight_layout()
    plt.show()


def rotate_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],
        ],
        dtype=np.float64,
    )


def make_camera_poses():
    T_c1_to_w = np.eye(4, dtype=np.float64)

    R_c2_to_w = rotate_y(np.deg2rad(-30.0))
    t_c2_to_w = np.array(
        [0.5, 0.52, 0.47],
        dtype=np.float64,
    )

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

    return T_c1_to_w, T_c2_to_w


def render_camera_box_depth(T_c_to_w, title):
    depth_z = render_box_depth(
        K,
        width,
        height,
        T_c_to_w,
        box_min_w=[-0.5, -0.5, 3.5],
        box_max_w=[0.5, 0.5, 4.5],
    )

    points_c = depth_to_points(depth_z, K)

    print(title)
    print("valid pixels:", np.isfinite(depth_z).sum())
    print("depth min/max:", np.nanmin(depth_z), np.nanmax(depth_z))
    print("points shape:", points_c.shape)

    show_depth(depth_z, title=title)

    return depth_z, points_c


def main():
    rays = calculate_rays(K, width, height)
    print("right-center ray:", rays[120, 319])

    T_c1_to_w, T_c2_to_w = make_camera_poses()

    render_camera_box_depth(
        T_c1_to_w,
        "camera 1 box z-depth",
    )

    render_camera_box_depth(
        T_c2_to_w,
        "camera 2 box z-depth",
    )

    return 0


if __name__ == "__main__":
    raise SystemExit(main())

后续可以继续补的检查

下一步建议给这个实验增加断言，而不是只看图：

assert rays.shape == (height, width, 3)
np.testing.assert_allclose(rays[120, 160], [0.0, 0.0, 1.0])
assert rays[120, 319, 0] > 0
assert rays[239, 160, 1] > 0

再把 camera-space points 转到 world-space，检查命中的点是否确实落在 box 表面：

至少一个坐标接近 box_min 或 box_max
并且另外两个坐标落在 box 范围内

这样就能从“图看起来对”升级为“几何数值上可验证”。