“dToF 转深度图”听起来像一个格式转换，实际上至少有三个问题：传感器给的是什么距离？每个测量对应哪条光线？最终要生成哪个相机坐标系下的图？

本文接着 小孔成像与相机内参 往下讲。没有绑定某一款硬件；具体设备的单位、有效状态、光线方向和 SDK 输出定义，必须以该设备文档为准，不能根据 depth、distance_mm 这样的变量名猜。

1. dToF 测的是飞行时间，但 SDK 不一定输出原始斜距

dToF 是 direct Time of Flight，即直接飞行时间测距。在发射与接收中心近似重合、往返路径相同，并已扣除电子延迟等偏置的简化模型下：

$$r\approx\frac{c\,\Delta t}{2}$$

其中 $c$ 为光速，$\Delta t$ 为往返传播时间，$r$ 为到反射点的距离。例如往返约 10 ns 对应约 1.5 m。发射与接收中心明显分离时，传播时间对应两段路径之和，应依赖设备的标定模型，而不是无条件把一半路径当作接收中心到点的距离。ST 官方 dToF 介绍第 11 页 给出了基本往返时间关系。

具体实现可能对光子到达时间建立直方图、估计峰值并输出置信度。测量还受到反射率、环境光、多径、串扰与混合目标等影响，并不是每个输出数值都天然可靠。多区域 dToF 的一个实例是 ams OSRAM TMF8821。

这里要把三个维度分开：

ToF 是原理，LiDAR 常指主动光学探测系统，不能简单把两者当成互斥类别。反过来，名字叫“深度相机”也不代表它是 ToF；例如 RealSense D435 的官方技术类型为立体视觉，本站 D435i 笔记中的双目原理不能直接搬来解释 dToF。

一个真实反例：distance_mm 已经可能是 Z

ST 员工对 VL53L5CX 的说明指出，该器件内部已经做了 radial-to-perpendicular 转换：正对一个垂直于光轴的平墙时，各区域输出应近似对应相同的垂直距离，而非边缘更长的原始斜距。见 ST 关于 VL53L5CX 输出含义的说明。

因此，对这类已转换输出再乘一次 $\cos\theta$，反而会把正确平墙“压弯”。这是具体型号的反例，不是所有 dToF 都输出 Z 的保证。

2. range 与 Z：同一个点的两种距离

仍使用相机 optical 坐标：$X$ 右、$Y$ 下、$Z$ 前。空间点为 $P=(X,Y,Z)$，则：

$$r=\sqrt{X^2+Y^2+Z^2},\qquad Z=r\cos\theta$$

$\theta$ 是光线与正光轴的夹角。光轴上二者相同，边缘则一般有 $r>Z$。

考虑一面位于 $Z=2$ m 的正对平墙：

• 光轴上的点：$r=2$ m，$Z=2$ m。
• 水平离轴 45° 的点：$X=2,Y=0$，$r=2\sqrt{2}\approx2.828$ m，但 $Z$ 仍是 2 m。

如果把第二个点的 $r=2.828$ 当成 Z 反投影，会得到 $(2.828,0,2.828)$，而不是 $(2,0,2)$。越偏离光轴，几何越被拉长。

反过来，若 SDK 已经输出 Z=2 m，却误当成 range 再乘余弦，就得到 Z≈1.414 m。没有转换、重复转换，都会错。

正对平墙是一个很有用的排错实验，但要避开墙边、无效状态、多径和低信噪比区域；它是验证文档理解的辅助证据，不是替代型号文档的万能判据。

3. 先选输入分支，再恢复三维

3.1 已知单位光线方向时，公式最通用

设厂家标定得到某个区域或测量点的单位方向：

$$\hat d=(d_x,d_y,d_z)^T,\qquad\|\hat d\|=1$$

则输入不同，公式也不同：

注意这里 $d_z$ 与 $Z_S$ 必须针对同一条轴、同一个坐标系定义。如果设备坐标不是本文 optical 约定，要先明确它的坐标映射。

3.2 无畸变针孔图像是一个特殊情况

若输出确实对应已知 $K$ 的无畸变针孔像素网格：

$$q=\begin{bmatrix}x_n\\y_n\\1\end{bmatrix} =\begin{bmatrix}(u-c_x)/f_x\\(v-c_y)/f_y\\1\end{bmatrix},\qquad \hat d=\frac{q}{\sqrt{1+x_n^2+y_n^2}}$$

于是：

$$\boxed{P=Zq},\qquad \boxed{P=r\frac{q}{\|q\|}},\qquad \boxed{Z=\frac{r}{\sqrt{1+x_n^2+y_n^2}}}$$

千万不要把 $q$ 当单位向量直接写成 $P=rq$。此外，原始畸变像素必须先经过适合该设备的去畸变/反投影模型。

3.3 8×8 阵列不等于一张普通相机的小图

多区域 dToF 的一个格子通常覆盖一束有面积的角度范围，而不是无限细的一根理想光线。区域内存在前后两个物体时，输出还受峰值选择、回波和固件处理影响；把数值放在区域中心射线上，只是一种代表点近似。

不要仅凭“8×8、总 FOV 多少度”就断言每格等角度，或反推出一套准确的针孔 K。优先使用厂家的每区方向表、畸变模型或 SDK 反投影。

同理，按方位角/仰角排列的球面 range image 与按 $X/Z,Y/Z$ 排列的透视图不同。若某系统将方位角 $\alpha$ 从正 Z 轴向右计，仰角 $\beta$ 向上计，那么在本文右、下、前坐标中，一种明确约定的单位射线为：

$$\hat d=\begin{bmatrix}\cos\beta\sin\alpha\\-\sin\beta\\\cos\beta\cos\alpha\end{bmatrix}$$

实际设备的零角、正方向和通道角度可能不同，不能直接套用。更不能把球面图 resize 一下就称作透视深度图；应恢复三维后重新投影。PCL 的 RangeImagePlanar 文档 也区分了平面投影与球面投影。

4. XYZ 投到目标深度图：不只是改一个矩阵尺寸

假设已有源坐标系点 $P_S$，目标是某个相机 C 的深度图。正确顺序为：

1. 根据设备的状态、置信度、量程和单位筛掉无效观测。
2. 通过外参变换到目标相机：$P_C=R_{CS}P_S+t_{CS}$。
3. 丢弃非有限值及 $Z_C\le z_{\min}$ 的点，避免投影到身后或除以接近零的数。
4. 使用与目标图像模式匹配的 K 和畸变模型，得到像素 $(u,v)$。
5. 做目标图像边界判断，再离散化成像素索引。
6. 同一像素有多个候选点时，保留最近的正 $Z_C$，同时保留有效性掩码。

这里输出的是目标相机的 Z，不一定等于源设备的 range，也不一定等于源坐标系的 Z。ROS 标准浮点深度采用沿相机 Z 轴的米制深度，参见 REP 118。具体 SDK 的其他编码仍要单独确认。

4.1 带坐标轴变换的完整例题

源坐标为“前、左、上”，目标相机坐标为“右、下、前”，暂设两者原点重合、安装朝向没有额外旋转：

$$R_{CS}=\begin{bmatrix}0&-1&0\\0&0&-1\\1&0&0\end{bmatrix}, \qquad t_{CS}=0$$

这个 R 只是例题的轴约定变换，不是可用于真实硬件的通用外参。

对源点 $P_S=(5,-1,0.5)$ m：

$$P_C=(1,-0.5,5)\,\mathrm{m}$$

使用 $f_x=f_y=600,c_x=320,c_y=240$，得到：

$$u=600\frac{1}{5}+320=440,\qquad v=600\frac{-0.5}{5}+240=180$$

该像素应写入 Z=5 m，而非 $r=\sqrt{26.25}\approx5.12348$ m。

若另一点为 $(2,-1,10)$（目标相机系），它也投到 $(440,180)$。两者发生碰撞时留下 Z=5，而不是平均成 7.5。较小深度更新也是 Open3D 点云投影内核 中采用的基本处理方式。

4.2 像素离散化与无效值必须有明确约定

本文规定像素中心为整数，采用最近像素投影：

$$-0.5\le u<W-0.5,\quad -0.5\le v<H-0.5$$

$$i=\lfloor u+0.5\rfloor,\qquad j=\lfloor v+0.5\rfloor$$

这样先检查边界，再取整，输出索引一定合法。右、下边界不包含在内。若某库使用 floor(u)、不同的采样中心或边界规则，应整体匹配该库，而不是混用本文的边界与另一种取整方式。

没有点落入的像素用 NaN 或显式 invalid mask 表示。不能把“无观测”当成“距离 0 米”；展示时可以着色为黑色，但数值语义仍要区分。浮点 NaN 与某些 16 位格式中的 0 无效值，转换时也要显式处理。

4.3 Z-buffer 解决不了所有遮挡

取最小 Z 只解决已采到的点之间的投影碰撞。如果前景没有采到点，背景仍可能穿过空隙；不同传感器存在基线时，还有源视角看得到、目标相机却被遮挡的区域。

因此“已投影”“真实观测”“插值补洞”“遮挡推断”应分开记录。把前景 1 m 与背景 5 m 双线性平均成 3 m，会制造一个本不存在的表面。最近邻不会混合数值，但也可能复制错误表面；没有任何简单缩放方法能自动解决深度边界问题。

5. FOV 到底由什么决定

FOV 是视场角，不是分辨率。对于一个已经明确的无畸变针孔图像，它由像素范围和 K 一起决定。

在本文像素中心约定下，横向外边缘对应的方向角为：

$$\theta_L=\arctan\frac{-0.5-c_x}{f_x},\qquad \theta_R=\arctan\frac{W-0.5-c_x}{f_x}$$

$$\boxed{\mathrm{HFOV}=\theta_R-\theta_L}$$

同理，将 $W,c_x,f_x$ 换成 $H,c_y,f_y$ 得到垂直 FOV。这里的角度来自像平面横/纵截面；目标点的矩形透视范围由 $X/Z$、$Y/Z$ 判断，不能用球面仰角 $\operatorname{atan2}(Y,\sqrt{X^2+Z^2})$ 的固定上下界直接替代。

若主点严格位于几何中心 $c_x=(W-1)/2$，左右对称，才简化为：

$$\mathrm{HFOV}=2\arctan\frac{W}{2f_x},\qquad f_x=\frac{W}{2\tan(\mathrm{HFOV}/2)}$$

FOV 输入三角函数前要从度转成弧度。仅有一个对角 FOV，通常不足以确定 $f_x,f_y,c_x,c_y$；还需要像素尺度、主点、投影模型等假设。真实畸变镜头、多区域 dToF 或扫描传感器的覆盖范围，也不能只靠这组针孔公式描述。

同一视场将 640 列重采样成 320 列，K 也相应更新，视场可以不变；从 640 列只保留中间 320 列，视场则缩小。两者虽然输出尺寸相同，几何含义完全不同。

6. ROI 裁剪：主点平移，焦距不变

从原图取整数 ROI：横向索引 $[x_0,x_0+w)$，纵向索引 $[y_0,y_0+h)$。例如 NumPy 中：

cropped = image[y0:y0 + h, x0:x0 + w]

原像素中心在新图的坐标为 $u'=u-x_0,v'=v-y_0$。代入投影式可得：

$$f'_x=f_x,\quad f'_y=f_y,\quad c'_x=c_x-x_0,\quad c'_y=c_y-y_0$$

裁剪没有改镜头，只是换了像素原点与保留范围。主点可能偏到一侧，甚至落在裁剪图外；这本身并不意味着内参错误。

对 640×480 图像，取 $f_x=600,c_x=320$，只裁左右、保持高度：

微小不对称来自例题主点 320 与图像几何中心 319.5 的差别。这里不是用视场的像素比例硬乘角度，而是对裁剪后的左右边缘分别取反正切。

为什么“裁一半宽度”不是“FOV 减半”？对严格居中模型，宽度保留比例为 $\alpha$ 时：

$$\mathrm{HFOV}'=2\arctan\left(\alpha\tan\frac{\mathrm{HFOV}}{2}\right)$$

它是正切与反正切关系，而非角度线性关系。越广角，线性近似越容易产生明显偏差。

还有两种名字相近但含义不同的裁剪：

• 图像 ROI：从已有图像里去掉部分像素，按上式更新主点。
• 三维点云按目标相机视场筛选：先将点变到目标相机系，再投影并判断边界。若 dToF 与目标相机有平移，不能只裁源传感器的角度来代替，近距离视差会使结果不同。

7. Resize 与 letterbox：除了比例，还有半像素约定

7.1 先写坐标映射，再更新 K

设先裁剪，再按比例 $s_x,s_y$ 缩放，最后左右/上下补边，其中左侧 padding 为 $p_x$、顶部为 $p_y$。如果使用简单坐标映射：

$$u'=s_x(u-x_0)+p_x,\qquad v'=s_y(v-y_0)+p_y$$

则：

$$f'_x=s_xf_x,\quad f'_y=s_yf_y, \quad c'_x=s_x(c_x-x_0)+p_x, \quad c'_y=s_y(c_y-y_0)+p_y$$

其本质是 $K'=AK$，其中 A 是同一个像素坐标仿射变换。不能用完整 padded 宽度除原宽度来误算缩放倍率；缩放比例与补边是两步。

7.2 常见的中心对齐重采样需要额外 0.5

很多缩放实现以像素中心对齐采样。例如 OpenCV resize 实现 中常见的采样位置关系，对应如下前向坐标变换：

$$u'=s_x(u-x_0+0.5)-0.5+p_x$$

因此应使用：

$$\boxed{c'_x=s_x(c_x-x_0+0.5)-0.5+p_x}$$

纵向同理，焦距仍为 $s_xf_x,s_yf_y$。若使用 align_corners、不同采样模式或厂家图像管线，映射可能不同；这不是必须一律手工加减 0.5 的命令，而是要匹配真实的像素映射。

这里改用相机基础篇的点 $P_C=(0.2,0.1,2)$ m，其原图投影为 $(380,270)$，不是本篇第 4 节的点。原图先裁 $(160,120,320,240)$，再缩小到 160×120，不补边：

两种结果的差别来自明确的坐标约定，不是空间点变了。对中心对齐缩放，用本文像素外边缘计算的 FOV 保持不变。

再举 letterbox：640×480 等比例缩到 320×240，放入 320×320 画布，上下各补 40 像素。中心对齐时 $f'_x=f'_y=300,c'_x=159.75,c'_y=159.75$。上下 padding 没有真实测量，必须保留 invalid mask，不能因为 K 能描述那些方向就认为传感器看到了更多区域。

7.3 图像尺寸缩放，不等于深度数值缩放

像素从 640×480 变成 320×240，某个点的真实深度 2 m 仍然是 2 m。要变化的是该点的位置和内参，不是米制深度乘 0.5。

还需注意：

• 对深度图做普通双线性插值可能混合前后景；应根据任务选择带有效性判断、边界意识的处理，并标记估计值。
• 从低分辨率 dToF 放大到高分辨率不会创造真实测量或提升原始角分辨率。
• undistort、rectify、align 到 RGB 可能涉及非线性重映射或不同相机视点，不能只用一个 crop/resize 仿射 A 代替。
• 在 ROS 中，原图 K、矫正图 P、ROI、binning 的约定应与驱动和 image_geometry 的处理一致，避免重复扣 ROI 或重复缩放。参见 CameraInfo 定义。

8. 一份不用硬件也能运行的参考实现

下载 camera_geometry_demo.py。它只依赖 Python 3.9+ 标准库，包含 range/Z 反投影、FOV、crop/resize 内参更新、坐标变换和最近 Z 投影。

python3 camera_geometry_demo.py --self-test

文件中的投影核心是：

# 已经先检查设备状态、置信度和单位；目标是无畸变针孔图。
point_c = transform_point(point_s, rotation, translation)
# 丢弃非有限值和 Z <= z_min 后：
u, v = camera.project(point_c)
if -0.5 <= u < camera.width - 0.5 and -0.5 <= v < camera.height - 0.5:
    # 先通过连续坐标边界检查，再防范浮点加法将末端合法值舍入到 W/H。
    col = min(camera.width - 1, floor(u + 0.5))
    row = min(camera.height - 1, floor(v + 0.5))
    z = point_c[2]
    if isnan(depth[row][col]) or z < depth[row][col]:
        depth[row][col] = z

以下例子可以在下载文件所在目录运行：

from camera_geometry_demo import CameraModel, rasterize_z

camera = CameraModel(640, 480, 600, 600, 320, 240)
rotation = ((0, -1, 0), (0, 0, -1), (1, 0, 0))
points_source = [(5, -1, 0.5), (10, -2, 1)]
depth = rasterize_z(points_source, camera, rotation)
print(depth[180][440])  # 5.0，两个点落到同一像素，保留更近的 Z

cropped = camera.crop(160, 120, 320, 240)
resized = cropped.resize(160, 120, center_aligned=True)
print(resized.fx, resized.cx)  # 300.0 79.75
print(cropped.fov_deg())

这是教学实现，不是某款 dToF 驱动：没有替你估计外参、读取置信度、处理镜头畸变或做运动补偿，也没有进行深度补洞。输入点必须先完成相应设备的数据校验。大规模实时点云可换成向量化或并行实现，但应保留这里可验证的坐标、单位和边界约定。

9. 真正接硬件时的排错顺序

时间同步值得单独强调：匹配相近时间戳不等于修正了设备时钟偏差，也不等于消除了曝光或扫描期间的运动。若设备是扫描式输出，需要确认点级采集时刻；非扫描多区域器件则按其实际采集方式处理，不应无条件套上扫描雷达的 deskew 流程。

接入前建议把以下信息写进配置或数据说明：型号与 SDK 版本、输出是 range 还是 Z、单位与无效状态、区域/像素到射线的标定、坐标轴、外参方向、目标图像模式、时间戳来源、crop/resize/padding 顺序。这样后续换设备、改分辨率时，不用重新猜一遍。

10. 用四个问题检查是否真的理解

1. 某 dToF SDK 已输出垂直深度，边缘像素还要乘 $\cos\theta$ 吗？
2. 将宽度从 640 缩到 320，深度 5 m 应变成 2.5 m 吗？
3. 偏心 ROI 宽度相同，FOV 一定与中心 ROI 相同吗？
4. 一张深度图的黑色区域能直接认定为近距离障碍物吗？

答案依次是：不应重复转换；米制深度不跟图像尺寸缩放；不一定，应该从左右边缘方向重新算；不能，先区分无效值、无观测和显示配色。

把这篇压缩成一条实践主线就是：确认距离语义 → 用正确光线恢复三维 → 变换到目标坐标系 → 投影与有效性处理 → 图像预处理时同步更新内参。

回到 计算机视觉基础分类，或阅读 相机内参与小孔成像 和 RealSense D435i 工程入门，对照不同测量原理下相同的投影几何。