深度相机与 3D 定位：一个像素加一个深度，算出它离你多远
前面九个实验都在二维平面上工作：框、掩码、点、ID —— 全都在说「在画面哪个位置」。可机械臂要抓杯子，问的是「它在我前方多少米、偏左多少」。这一页补上第三维：深度图给你距离，内参给你方向，两个一乘就是三维坐标。
01目标：点画面任意一点，报出它离我多远
- 说清深度相机的输出是什么单位、什么类型，以及「0 代表无效」这个约定为什么必须记住
- 跑通双栏观察器（左 RGB / 右深度伪彩），并在深度图上点出
z = 309 mm (0.309 m) - 手算出同一个点的
xyz三元组，与程序结果对上；再用错一次fx看它偏多少
fx/fy/cx/cy 把像素变成米。难点：直觉是「深度就是一个距离」，实际是沿光轴的距离 z，不是到相机的直线距离；而且点越靠画面边缘，同一个 z 对应的实际距离越长。
在深度图窗口里点一下。深度来自 /ascamera_hp60c/camera_publisher/depth0/image_raw。
沿光轴 0.309 米、右偏 0.046 米、左偏 0.039 米 —— 可以直接喂给机械臂或导航。
02问题：只知道「在哪」，不知道「多远」
机械臂要抓桌上的杯子。检测框会告诉你「杯子在画面右下角」—— 可「右下角」是多少米？左偏 5 厘米还是 15 厘米？
普通相机给不出答案：一张照片里，近处的大杯子和远处的小杯子可以长得一模一样。距离这个信息在拍照那一刻就被压掉了。深度相机把它补回来：每个像素除了颜色，还多一个「这一点有多远」。
| 对比 | 只有 RGB（黑盒） | 加上深度（白盒） |
|---|---|---|
| 这个杯子在哪 | 画面第 260 列、第 324 行 | ？ —— 还要知道这一点的距离，才能变出 (x, y, z) 米 |
| 为什么近的大远的小 | 知道这个现象，算不出来 | ？ —— 距离一除，就能反推出真实尺寸 |
| 能不能去抓 | 不能，机械臂要的是米 | ？ —— 还需要相机的三个内参把像素换成方向 |
三个问号拆成三个具体问题：
① 深度图里那个数是什么？为什么是 309 而不是 0.309？
② 同样是 0.309 米，为什么点画面边缘时算出来的坐标差很多？
③ 内参 fx / fy / cx / cy 各自管什么？改错一个会怎样？
03现象：左彩图右伪彩，点一下报 309 mm
你会看到：一个双栏窗口，左栏彩色图、右栏深度伪彩（近处红黄、远处蓝），顶部写着 Click depth image to read depth。在右栏点一下，画面左下角与终端同时报出 z 和 xyz。
z = 309 mm 与 xyz 三元组同时落在画面左下角怎么跑出来：深度必须从话题取（文件模式需要成对的 RGB 图 + uint16 深度 PNG，仓库未附带）。先起驱动再跑程序，两个终端 ROS_DOMAIN_ID 一致（N 取 0~101，各设备不同）。端口用 8083 避开前面实验的 8080。
终端一 · 起相机驱动
source /opt/ros/humble/setup.bash && source ~/ros2_ws/install/setup.bash export ROS_DOMAIN_ID=N ros2 launch ascamera hp60c.launch.py
终端二 · 跑观察器
source /opt/ros/humble/setup.bash && source ~/ros2_ws/install/setup.bash export ROS_DOMAIN_ID=N cd ~/ros2_ws/src/xflz_eai_car1/vision/08_depth_3d_detection python src/app.py --ros --port 8083
🔍 看什么：三处。
- 右栏颜色不是「好看」而是「读数」 —— jet 伪彩把距离映射成颜色：近处偏红黄、远处偏蓝。台阶、桌沿这些跳变会看到明显的色带边界。
- 深蓝近黑有两种含义 —— 一是深度值
0（没测到：反光、太近、太远都可能是 0），二是恰好是画面里最近的那片。因为normalize_depth()按 min-max 拉伸，最小值被映射成 0，和无效值撞成同一个颜色。要看颜色猜深度，这里会踩坑。 - 读数有两组 ——
z是沿光轴的距离，xyz才是三维坐标。别把z当成「到相机的直线距离」。
这就是本实验最终要呈现的现象。窗口点击在板子桌面或 noVNC 里做；SSH 里点不了窗口，所以第 05 节给了「用 MJPEG 读数 + 手算对照」的替代路径。
04原理：三个公式：毫米转米、取该点深度、反投影成 xyz
像素告诉你方向（这个点在画面哪个角度上），深度告诉你距离（沿光轴多远），内参把这两件事合成一个三维坐标。
打个比方：像站在窗前看远处的楼 —— 你眼睛能指出「在左前方 10 度」，但不知道多远；一旦有人说「那楼 30 米」，位置就定了。 揭个盖：深度给的是正前方 30 米（沿光轴），不是斜着量 30 米。所以点越靠画面边缘，同样 30 米深度对应的实际距离越远 —— 这也正是「边缘会偏」的原因。
三个步骤回答第 02 节的三问：① 数是毫米 → 步骤 1；② 边缘为什么偏 → 步骤 3；③ 内参各管什么 → 步骤 3 的四个字母。
cx / cy 只是把原点挪到光轴上。上面用
config.cam3d 的 fx=608.40 / cx=328.30 手算，是为了让你能自己复现。板上 ROS 模式实际用的是话题 camera_info 里的内参（实测 fx≈592.90、cx≈335.43），两者不同，所以手算值和实测 xyz 会差几毫米 —— 这不是算错了。1深度图里那个 309 是什么单位？
吃什么：话题 depth0/image_raw（16 位单通道）。做什么：读成 uint16 数组。吐什么：一张 640×480 的整数图，单位毫米。
为什么用整数毫米而不是浮点米：省一半带宽、精度够（1 毫米）。代价是范围受限（最大约 65 米）且0 必须当无效值 —— 反光、太近、超量程都会给 0。如果直接把它当 0 米参与计算，会得到一堆「贴着镜头的点」，看起来像一堆噪点。
动它会怎样：depth.scale 写错（比如写成 1.0）→ 所有距离放大 1000 倍；invalid_value 没判 → 无效区会污染结果。
2为什么 309 要除以 1000？
因为后面所有公式都按米算。这一步看着无聊，却是最常见的单位事故来源：
- 忘了除 → 坐标全变成毫米量级的「几百」，机械臂直接撞上去。
- 除了两次 → 距离变成 0.0003 米，全都贴在镜头上。
- 以为
scale是「像素当量」→ 与内参混为一谈，两个都错。
动它会怎样：scale = 0.001 是这套数据的约定；换相机一定要重新确认，不要沿用。
3fx / fy / cx / cy 到底在干什么？
这是第 02 节第二、三问的核心。把相机想成一个「小孔」：光线从物体穿过小孔打在传感器上。于是：
fx：水平方向的「焦距（以像素为单位）」。它决定了1 米远处 1 米宽的东西占多少像素。公式里它在分母：同一段像素偏移u - cx，fx越大算出的 x 越小。fy：垂直方向同理。多数相机fx ≈ fy，但不完全相等。cx / cy：光轴打在画面上的那个像素（通常接近画面中心）。它是「零点」——u = cx时 x 正好是 0。
为什么边缘会偏：u - cx 越大（越靠边），同样的深度下 x 越大；而深度 z 在斜看的地方也更不可靠（深度相机是沿光轴测的）。所以边缘点的三维坐标误差天然比中心大。
动它会怎样：把 fx 从 608 改成 300 → 所有 x 放大一倍（画面被「当成更广角」）；改 cx → 整体左右平移。这就是第 06 节要拖的东西。
z 是毫米转米的产物，xyz 是内参的产物 —— 两个数各管一段，缺一段就没有三维。先认识 5 个词
每个像素存「沿光轴多远」的图，本仓是 uint16 毫米。什么时候失效：反光面、透明玻璃、超出量程的位置会给 0。
fx / fy / cx / cy 四个数，描述这台相机的几何。什么时候失效：换了相机、改了分辨率不重标定，坐标就会系统性偏移。
由「像素 + 深度 + 内参」算出三维坐标的过程。什么时候失效：深度本身错（边缘、反光）时，算得再准也是错的三维点。
物体在正前方方向上离相机多远，不是直线距离。什么时候失效：把它当直线距离用，边缘点的距离会被低估。
把距离映射成红黄蓝的颜色，便于肉眼判断远近。什么时候失效：颜色只是可视化，绝不能拿颜色反推距离。
什么时候会翻车：五个真实边界选读 1 分钟
- 玻璃 / 镜面 / 黑色绒布 —— 深度直接给 0 或乱跳。这几类表面是深度相机的天敌。
- 太近（小于约 0.2 米） —— 超出近端量程，回到 0；表现为「明明有东西却没有深度」。
- 画面边缘 —— RGB 与深度由两个镜头拍摄，边缘存在视差；同一像素在两张图里并非严格对应，边缘误差最大。
- 分辨率改过却没重标内参 —— cx/cy 应与分辨率成比例变化，直接沿用 640×480 的值会整体偏移。
- 窗口缩放后点击 —— 显示做了
display_scale: 0.666，点击坐标必须除以它才能还原到图像坐标；不做这一步，点哪都偏。
| 步 | 数据变成什么 | 为什么非它不可 / 少了会怎样 | 谁在干 | 动参数会怎样 |
|---|---|---|---|---|
| 1 | 话题帧 → 640×480 uint16 毫米 | 不判 0 就得到一堆「贴脸的假点」 | depth.load | invalid_value 决定哪些算无效 |
| 2 | 整数 → 米（× 0.001） | 不换算，后面所有公式量级全错 | depth.scale | 改成 1.0 → 距离放大 1000 倍 |
| 3 | (u, v, z) + 内参 → (x, y, z) 米 | 没有内参，像素只是「格子号」，不是方向 | depth.backproject | fx↑ 同样的 u 偏得更小；cx 移 → 整体平移 |
这张表只有三行，但第 5 列每一格都能在第 06 节拖出来：这是本页唯一能把公式变成手感的地方。
05实操：双栏 / 点击读深度 / 手算对照
先看命令与原理的对应关系。
| 环节 | 函数（文件） | 吃哪个参数 | 对应原理第几步 |
|---|---|---|---|
| 取 RGB / 深度 | Ros2Camera（ros2_source.py）订阅两个话题 | --ros source.mode | 链外 |
| 深度加载与伪彩 | depth.load / depth.colorize（depth.py） | scale invalid_value colormap | 步骤 1 |
| 取点与反投影 | depth.read_at / backproject（depth.py） | cam3d.* 四个内参 | 步骤 2、3 |
| 双栏合成与状态栏 | view.compose（view.py） | display_scale | 链外（显示） |
| 推流 | MjpegServer | server.port | 链外 |
-
起双栏，先看懂伪彩约 4 分钟
命令见第 03 节。浏览器打开
http://<板IP>:8083/depth.mjpg，把相机对着桌面、墙面、再对着自己的手 —— 观察伪彩颜色的变化，以及什么时候出现大片黑色。判据：双栏都在动、伪彩有层次、手靠近时颜色变暖（红黄）。
-
点一下，读出两个数约 5 分钟
窗口点击要在板子桌面（或 noVNC
http://<板IP>:6080/vnc.html）里做：cd ~/ros2_ws/src/xflz_eai_car1/vision/08_depth_3d_detection python src/app.py --ros --display
Click (260, 324): z = 309 mm (0.309 m) ← 有内参时同一行后面还会跟 xyz=(…, …, …) m ← 点在无效区会提示 invalid点同一个物体的中心和边缘各一次，比较两个
z—— 边缘往往跳变更大，这是视差与边缘深度不可靠的直接体现。 -
手算对照：把公式真的代一遍约 8 分钟
用第 02 节那三个公式，拿你的读数手算
x / y，再和程序的xyz比。差在几毫米以内算对得上。python3 -c "u,v,z=260,324,0.309; fx,fy,cx,cy=608.39958,611.20714,328.30096,226.46483; print(round((u-cx)*z/fx,4), round((v-cy)*z/fy,4), z)"-0.0347 0.0493 0.309然后故意改错一次：把
fx改成 300 再算一遍 —— x 变成 -0.0704，差不多翻倍。这一步做完，第 06 节就不用猜了。
z = 309 mm 与 xyz 三元组，而且你能自己用三个公式复现它。两处对上了，说明深度读取、单位换算、反投影三段你已完整走通。你该看到的画面
近处红黄、远处蓝，色带边界对应物体边界，点击能读出不跳变的 z。
玻璃、镜面、太近太远都会是 0 —— 不是「距离为零」。
这一点的深度是 0，换一个点（或换一块表面）再试。
先确认 display_scale 的还原，再核 cx / cy。
踩坑速查
| 现象 | 原因 | 怎么办 |
|---|---|---|
| 点哪都偏一大截 | 窗口显示缩放后没还原坐标 | 点击坐标除以 display_scale（0.666）再读深度 |
| 点出来的 z 是 0 / 提示 invalid | 该像素深度为 0（反光、太近、超量程） | 换一块表面或稍微挪动相机；代码里按 invalid_value 判掉 |
| 距离差了 1000 倍 | 毫米没转米，或转了两次 | 确认 depth.scale = 0.001 只用一次 |
| 坐标整体左/右平移 | cx / cy 与当前分辨率不匹配 | 用与分辨率配套的内参；改分辨率必须重标 |
| RGB 与深度对不齐 | 两个镜头有视差，边缘最明显 | 接受它：这是硬件特性；需要精确对齐得做深度-彩色配准 |
| SSH 里点不了窗口 | 没有显示服务器 | 用 noVNC，或走第 05 节的手算对照路径 |
进阶 1 · 把一块区域的平均深度算出来约 20 分钟
单点深度容易被噪声影响。取检测框中心的一个小窗口（如 11×11），把非零值求中位数，作为「这个物体的距离」—— 这是抓取前最常用的一步平滑。
用中位数而不是平均值，是因为窗口里常有少量 0 或离群值，平均值会被拉偏。
进阶 2 · 把检测框升级成三维框约 25 分钟
框底边中点 + 该处深度 → 物体在桌面上的落点 (x, z)；框高 × 深度 / fy → 物体的真实高度（米）。这样机械臂就知道「在哪抓、抓多高」。
这就是「2D 检测 + 深度」最经典的组合，也是本课程从「看见」走向「抓取」的那一步。
🧪 Vibe Coding · 自己把这段代码读明白约 20 分钟
你要产出三样：模块职责表 → 控制流流程图 → 时序图。
提示词 1 · 模块职责分析
请阅读这个深度观察器项目的模块：src/app.py、src/depth.py、src/view.py、src/config.py、src/ros2_source.py。 它们共同完成「订阅 RGB 与深度 → 双栏显示 → 点击读深度并反投影」。 请输出一张表，每行一个模块，四列：模块名 / 一句话职责 / 被谁调用 / 从哪个配置项取值。 关键函数标出行号；不确定就说明，不要猜。
提示词 2 · 控制流流程图
请画一张「一次点击」的流程图：从窗口鼠标事件开始，经坐标还原（除以 display_scale）、取深度值、判无效、单位换算、反投影，直到把结果写进状态栏。 每个框写函数名，判断框写条件（尤其是 depth == invalid_value 这条分支）。
提示词 3 · 时序图
请为 ROS 模式画一张时序图，参与者：ascamera 节点（两个话题）、Ros2Camera 回调、主循环、MjpegServer、用户点击。 标出 RGB 与深度两路各自的频率、主循环如何把它们配对成同一帧、点击事件在哪个线程里被处理。
自查三问：能指出坐标还原发生在哪一行？能说清 RGB 与深度不是同一时刻的帧吗？无效值分支在流程图上有没有画出来？
三处必然的差异：
| 差异点 | AI 版常见写法 | 工程版（本仓） | 为什么工程这么写 |
|---|---|---|---|
| 单位处理 | 直接拿 uint16 当米用，或忘记转 | 统一乘 scale = 0.001（depth.py） | 1000 倍量级错误会直接撞机，必须在入口处一次转干净 |
| 无效值 | 只判 d > 0 就完事，或干脆不判 | 按 invalid_value: 0 显式判并在界面上提示 invalid | 0 是最常见的值（反光/超量程），静默当 0 米会产出假点 |
| 点击坐标 | 直接把鼠标坐标当图像坐标 | 先除以 display_scale 还原 | 窗口经过缩放，不还原就会「点哪都偏」，且缩放比例改了更乱 |
改一处、先预测再验证：把 cam3d.fx 乘以 2，先写下 x 会变成多少，再点一次对答案。
06互动：故意改错 fx，看坐标怎么一起偏
📌 这一节是「掌握检查」：亲手改一个参数，看现象是否朝你预期的方向变，还要能说出为什么 —— 说得出，才算掌握。
反投影只有三行公式，但「哪个数错会导致哪种偏」不亲手拖一遍是记不住的。这里把 u（点在哪一列）、z（深度）、fx（内参）都交给你。
u 决定视线方向，深度 z 决定沿光轴走多远，两线交点就是物体的 (x, z)。公式与源码一致：x = (u − cx)·z / fx。怎么读出规律：只动 u —— 视线角度变，x 线性变；只动 z —— 同一条视线走远，x 也线性变大（这就是「近小远大」的几何来源）；只动 fx —— 物体位置没动，坐标却在变，因为它改的是「这台相机怎么看世界」。
点正好在光轴上，物体就在正前方，只有 z 没有横向偏移。
边缘像素的横向偏移最大 —— 也是误差最大的地方。
相当于把相机当成「广角更多」，同样像素偏移对应更大角度。
同一角度看得更远，横向偏移自然更大 —— 这就是「远处同样偏角、横向差得多」。
07验证：三道小测
回页首对照「实验目标」：① 单位与 0 值约定说清了没有 → 试 Q2；② 双栏跑通、点出读数 → 回第 05 节自查；③ 手算对上、fx 改错看偏 → 试 Q1 与 Q3，并回第 06 节拖一遍。三条全 ✓ 就完成。
u=300, v=240, z=0.5 m，fx=600, fy=600, cx=320, cy=240，该点的 x 是多少？x = (u − cx)·z / fx = (-20) × 0.5 / 600 ≈ -0.0167 m。注意三件事：先减 cx、z 用米、fx 在分母。第三个选项是把像素差直接当米了，忘了除 fx、也没乘 z。invalid_value: 0 是这套数据的约定。把它当 0 米会算出一堆「贴脸的假点」，看起来像噪点爆炸。深度相机对玻璃、镜面、黑绒布本来就不擅长，0 是常态。fx 从 608 改成 304（其它不变），同一个像素的 x 会怎样？fx 在分母。它代表「焦距的像素当量」，越小意味着视场越广、同样像素偏移对应更大的角度。这也是为什么改了分辨率却不重标内参会整体偏 —— 相当于偷偷改了 fx / cx。08章节总结：像素给方向、深度给距离、内参给几何
这页讲了什么，它们怎么串起来
| 概念 | 它解决了什么 | 和别的关系 |
|---|---|---|
| uint16 深度图 | 给每个像素补上「多远」 | 上游来自深度相机话题，下游是反投影的输入；0 = 无效 |
scale = 0.001 | 毫米转米，统一量纲 | 漏乘/多乘都会造成 1000 倍事故 |
内参 fx/fy/cx/cy | 把「格子号」翻译成「方向」 | 决定同样像素偏移对应多少米；换相机/分辨率必须重标 |
| 沿光轴 z | 定义「多远」是沿正前方的距离 | 不是直线距离；边缘点的直线距离比 z 长 |
| 反投影 | 把 (u, v, z) 变成 (x, y, z) 米 | 整条 2D→3D 的最后一环，机械臂/导航的输入 |
| jet 伪彩 | 让远近肉眼可读 | 只用于显示，不能反推距离 |
display_scale | 把窗口点击坐标还原成图像坐标 | 纯显示层，但漏了它「点哪都偏」 |
② 直觉是读到 0 就是「贴着镜头」，实际是「没测到」，必须按无效值跳过；
③ 直觉是内参是标定课的事、跟应用无关，实际是改了分辨率不重标，坐标会系统性偏 —— 而且看起来「挺合理」所以更难发现。
②
invalid_value: 0 必须显式判，别用 > 0 顺手带过（0 之外还可能有超量程的哨兵值，换相机要重新看手册）；③ 端口：配置默认 8080（
depth.mjpg），同时跑其它实验时用 --port 8083 错开；④ 深度与 RGB 是两个镜头，边缘存在视差，不要指望同一像素在两图里严格对应。
前置依赖 · 缺一样就跑不起来
| 类型 | 具体项 | 缺了会怎样 / 怎么补 |
|---|---|---|
| 环境 | ROS 2 Humble 两级 source + 相机驱动（ascamera hp60c.launch.py），两终端 ROS_DOMAIN_ID 一致 | 起不来会打印 cannot open ros topic；未激活 venv 则 python: command not found |
| Python 库 | numpy、opencv-python、pyyaml、rclpy、cv_bridge、mjpeg_streamer | 缺 cv_bridge 无法把话题消息转成图像；缺 mjpeg_streamer 启动即退出 |
| 模型 / 资源 | 无模型（本实验不做识别）；需要深度相机在位，且 depth0/image_raw 有数据 | 相机不在位时两个话题都没有，画面全黑 |
| 配置 | config/depth3d_config.yaml：scale 0.001、invalid_value 0、display_scale 0.666、cam3d 四内参、source.mode ros | 内参与分辨率不匹配 → 坐标整体偏；mode 写 image 却没填路径 → 直接报错 |
| 显示 | 窗口点击需要桌面或 noVNC（http://<板IP>:6080/vnc.html） | 纯 SSH 会话点不了窗口；改用 MJPEG + 手算对照 |
这套代码用在哪儿
- 机械臂抓取：检测框中心 + 深度 → 桌面落点，直接作为抓取位姿的 x/y；再配上 z 决定下降高度。
- 导航避障：把深度图裁成前方扇区，取最小值当「最近障碍距离」，比激光雷达便宜得多。
- 体积/高度测量：物体真实高度 = 像素高度 × z / fy，配合深度就能量包裹、量料堆。
- 不该用它的场合：只需要平面内的位置（如二维码对准）就别上深度，多一路数据多一份不稳定。
x = (u − cx)·z / fx —— 三个数凑齐，二维才变三维。有了三维坐标，下一步就能把它变成动作：EXP-11 用骨架判定「举手/下蹲」，再翻译成两条速度指令。