机器人软件与具身智能 · 第三部分：视觉与三维感知 · 第 15 篇
前两篇确定了像素射线、深度和相机安装关系。本篇把这些关系用于整张深度图，再解释点云为什么需要滤波、体素化、平面估计和聚类。所有数值来自可运行的合成场景，不是八界实机采集结果。
八界准备收纳地面上的两个玩具。深度图已经到手，把每个像素反投影成三维点，好像就能得到两个物体的位置了。但真正生成点云以后，地面占了绝大多数点；玩具边缘混着无效深度；两个本来分开的物体，还可能被一串噪声点连成一大簇。
点云处理因此不是"把图像换成更立体的图片"。每一步都在改变下游可以使用的证据：反投影确定每个测量的空间位置，滤波决定哪些测量暂时不可信，降采样改变空间密度，分割再提出哪些点可能共享一个表面或物体的假设。
顺序和参数会改变结论。尤其要警惕：一个看起来整洁的点云，不一定比原始数据更适合抓取；一个轮廓完整的簇，也不一定就是一个独立物体。
图 1｜从深度观测恢复空间结构，再组织目标点集。基于八界 URDF 与作者提供的外观参考制作的概念示意，非实机拍摄；点云形状不代表设备测量结果。
1. 反投影以后，数据仍然属于某次观测
对已经与相应相机模型匹配的轴向深度 Z，针孔反投影是：
text
X_C = (u − cx) Z / fx
Y_C = (v − cy) Z / fy
Z_C = Z这里的 u、v 是这份深度所对应的像素位置。若使用彩色图上的掩码选择深度点，就要先确认深度已对齐到彩色像素网格，并采用对齐后适用的模型；原始畸变像素还要按对应畸变模型处理。把两个相同分辨率的数组直接按下标配对，并不能证明它们来自同一条光线。
八界公开 ROS 2 开发示例中，彩色图和 aligned_depth_to_color/image_raw 分别订阅。1 "aligned"说明这路数据的像素对应目标，但应用仍需要处理采集时刻、有效值、深度编码与单位。两路回调里保存的"最新消息"也可能不属于同一时刻，不能只看数组尺寸就跳过第 6、11 篇讨论的时间问题。
得到相机点 p_C 后，使用观测时刻的变换进入基座或地图：
text
p_B(t) = R_B_C(t) p_C(t) + t_B_C(t)一个完整点云对象最好能回溯到图像 ID、采样戳、相机模型版本与原像素索引。否则三维簇看起来有问题时，你无法知道它来自哪一片反光边缘，还是来自一个旧的检测框。
本篇 15_pointcloud.py 的 depth_to_points() 同时返回三维坐标和 (u,v) 索引，并排除非有限值和范围外深度。示例输入已经是米制 Z，不承担原始 ROS 字节流解码；需要解码行跨度、字节序与编码时，可以衔接第 13 篇。
2. 用一个能手算的场景检查坐标方向
教学深度图只有 64×48，内参为 fx=fy=100 px、cx=31.5 px、cy=23.5 px。背景平面深度为 1.0 m，两块可见物体表面的深度分别为 0.80 m 和 0.65 m。每个有效像素加入标准差 1.5 mm 的独立深度噪声，另外设置 20 个零深度和 20 个 NaN。
为了方便检查地面高度，假设相机正朝下，采用下面的教学变换：
text
R_B_C = diag(1, −1, −1)
t_B_C = (0.5, 0, 1.0) m这是绕 x 轴旋转 180°的合法旋转，行列式为 +1，不是随意翻转单个坐标轴的镜像。相机前方 Z=1 m 的点被变换到基座 z=0；Z=0.8 m 的表面在基座 z=0.2 m；Z=0.65 m 的表面在 z=0.35 m。这里没有使用八界实机外参。
例如像素 (20,24)、深度 0.8 m 的相机坐标为：
text
X_C = (20 − 31.5) × 0.8 / 100 = −0.092 m
Y_C = (24 − 23.5) × 0.8 / 100 = 0.004 m
p_B = (0.408, −0.004, 0.200) m这组简单数值比先打开一个炫目的三维窗口更适合检查单位和轴方向。如果你期待地面 z=0，却得到 z≈2 m，应先检查变换定义；如果位置放大一千倍，应先检查毫米与米，而不是调滤波阈值。
图 2｜3072 个像素中有 3032 个有效测量。右侧为生成点云的 x--z 投影；两个物体只用可见顶面模拟，未建模完整外壳。
把无效深度填成零再投影，会在相机原点附近制造一簇并不存在的点；变换以后，这簇点出现在相机安装位置附近。它可能被聚类算法当成一个真实目标。无效测量应保留其"未知"语义，不能为了数组没有 NaN 就变成确定坐标。
3. 点密度为什么随着距离改变
在局部正对相机、深度近似相同的表面上，相邻水平像素的空间间隔近似为：
text
ΔX ≈ Z / fx本例 fx=100 px，1 m 处相邻像素约相隔 10 mm；2 m 处约相隔 20 mm。面积上的采样密度还会随距离平方量级下降，倾斜表面又会进一步拉大某些方向的点间距。
于是同样的"半径 15 mm 内至少 5 个邻居"，在近处平面可能容易满足，在远处真实物体上却可能完全不满足。八界头部看地面与腕部近距离复核，不应默认使用同一组邻域阈值。需要结合成像尺度、目标细部、噪声和遮挡来选择，而不是把一个数字当成算法自带的物理常数。
距离同时影响误差。像素定位误差导致的横向偏差近似为 (Z/fx)δu；深度误差又会沿射线传播。一个远处稀疏点并不天然比近处稠密点更可疑，邻域规则应避免把"采样稀疏"直接等同于"错误"。
4. 体素降采样保留了什么，又丢掉了什么
体素网格把空间划分为固定大小的格子。给定网格原点 o、边长 s，点 p 的格子索引是：
text
k = floor((p − o) / s)将落在同一格中的点求均值，每个非空体素输出一个代表点。Open3D 的体素降采样采用这类分桶与均值代表的思路。2 本篇直接用 NumPy 实现，便于查看网格索引、每格计数和质心。
这里必须使用 floor。对于 x=-1 mm 和 x=+1 mm、体素边长 20 mm、原点为零，正确索引分别是 -1 和 0。若直接把浮点数转整数，负数向零截断，两个点都会变成索引 0，原点附近的格子边界就被破坏。
图 3｜20 mm 体素把本例 3032 个测量点变为 1349 个代表点。右侧展示 floor 与向零截断在负坐标处的差别。
为什么不直接把所有点都换成格子中心？因为格子中心不一定落在被观测表面上，而点的质心通常更贴近这些测量。不过"通常更贴近"也有条件：同一体素可能跨越两个深度层，平均以后会生成位于两表面之间的点。那里可能根本没有实体。
对于八界要抓的薄塑料片、玩具细杆或两个紧挨的部件，体素尺寸过大可能抹掉抓取所需的间隙。若两片表面距离为 8 mm，却使用 20 mm 网格，并不能期待降采样以后仍完整保留这个间隙。是否会落入同一格还依赖网格原点与物体方向，因此降采样数量也不是一个与坐标系无关的属性。
本例背景正好在 z=0 附近，噪声会让部分点落入 z 索引 -1，另一些落入索引 0。同一个看似平坦的面可能占据相邻两层体素，这也是不能只根据"平面面积除以体素面积"精确预测点数的原因。
质心能够降低某些独立随机噪声，但不自动消除深度偏置、相邻像素相关误差或混合表面。若下游还要把簇映射回图像，应保留每个体素对应的原始点索引，而不是只保留最后一个 XYZ 数组。
5. 离群点滤波是在应用假设
半径滤波的规则很直接：对每个点，数一数半径 r 内有多少其他点；少于某个数量就暂时排除。示例使用 45 mm 半径、至少 3 个邻居，邻居计数不包含自身。
代码在降采样后额外加入 3 个远离场景的孤立点，再执行过滤。3 个注入点全部被剔除，原有 1349 个代表点保留下来。这个结果验证了实现和当前输入条件，不证明所有真实飞点都能被这组参数去掉。
一条玩具天线末端也可能很孤立。它如果恰好没有足够邻居，会和飞点一起被删除。反过来，一团由反射造成的错误深度只要足够密集，就可能通过半径检验。
另一类统计滤波先计算各点的 k 近邻平均距离，再相对于总体分布判断异常。它适合某些密度相对一致的场景，但八界的近处机械结构、远处地面和被遮挡玩具可能属于不同采样密度。把所有点塞进一个全局距离分布，容易让稀疏但真实的区域吃亏。
因此滤波后要检查被删除的空间区域，而不是只看剩余点数下降了多少。针对当前操作任务，先裁剪合理工作区域、再处理局部密度，常常比盲目提高全局删除强度更容易解释。裁剪本身也要尊重目标与障碍信息的不同用途：不参与目标估计的点，未必可以从碰撞环境中删除。
6. RANSAC 怎样找到平面，又为什么会选错平面
单位法向 n 与标量 d 定义一个平面：
text
nᵀp + d = 0，且 ‖n‖ = 1
点到平面的距离 = |nᵀp + d|随机选择三个不共线的点 a、b、c，可用 (b-a)×(c-a) 得到法向，归一化后令 d=-nᵀa。再统计有多少点到这张平面的距离小于阈值。重复采样，保留支持最多的候选，然后用内点重新拟合。
三个点几乎共线时，叉积很小，法向对噪声极敏感；实现应跳过这类退化样本。重新拟合时，可把内点减去均值后做 SVD，最小奇异值对应的方向近似为平面法向。若内点本身几乎只构成一条线，仍然不足以稳定决定平面，这需要额外检查，不能只看返回了三个法向分量。
RANSAC 的迭代次数也能估算。设内点比例为 w，每次采 3 点，希望至少一次全为内点的概率达到 p，在独立抽样近似下：
text
N ≥ log(1 − p) / log(1 − w³)取 w=0.6、p=0.99，向上取整是 19 次。这是建立在内点率与有效模型假设上的采样估计，不是"运行 19 次就必然正确"。近共线样本、多个相似平面以及错误的 w 都会改变实际效果。
更重要的是，支持最多的平面不一定是地面。八界在家具旁观察时，一面墙或桌面可能占据更多点。要估计地面，可以在已知重力方向的参考系中约束法向、再检查高度和支撑范围；要估计桌面则应使用相应的高度和区域条件。没有正确参考系的"法向接近 z"没有物理意义。
本篇把点先变到教学基座坐标系，限制平面法向与基座 z 轴的夹角不超过 15°，距离阈值 6 mm，执行 120 次采样。拟合后也重新检查法向限制，避免细化过程破坏先验。Open3D 官方教程中的 segment_plane 给出了平面 RANSAC 的接口语义；本篇增加的方向约束是示例自己的选择。2
图 4｜本次运行拟合出 1230 个平面内点，留下 119 个物体表面点，最终形成 84 点与 35 点的两簇。
平面法向约为 (0.000176,0.000225,1.000000)，d 约为 -0.000087 m，与教学地面 z=0 一致。距离阈值 6 mm 约为输入深度噪声标准差的四倍，但经过反投影、体素化和姿态变换后，点到平面误差并不必然还是同一个高斯分布。因此这里是方便观察的实验参数，不是通用四倍法则。
7. 欧式聚类为什么会把两个物体粘在一起
把每个点看成图中的一个节点。如果两点距离小于 ε，就连接一条边。欧式聚类寻找这张图的连通分量：从一个未访问点出发，把它的邻居放入队列，再继续找邻居的邻居，直到没有新点。PCL 的聚类教程用近邻搜索和队列描述了这一过程。3
因此，同一簇内两个最远点的距离可以远大于 ε。算法只要求它们之间存在一条由短边连接的路径。
反例只需要一维坐标。第一组点从 0 到 0.10 m，间隔 0.02 m；第二组从 0.14 到 0.24 m，间隔相同。使用 0.025 m 邻域时，中间 0.04 m 的间隙让它们分成两簇。在 x=0.12 m 加一个点，左右相邻距离都变成 0.02 m，两组立即合并。
图 5｜欧式聚类的桥接反例。半径是局部连边条件，不能直接解释成物体之间必须保持的整体距离。
八界收纳时，残留地面点、连接两个玩具的细线、深度边缘飞点，都可能形成这种桥。把 ε 调小可以切断桥，也可能把同一个远处玩具切碎；把最小簇大小调大能去掉小块，却无法拆开已经连在一起的大簇。
DBSCAN 在邻域连通之外还引入核心点的密度条件，因此与本篇实现的简单连通分量并不相同。它能改变某些稀疏桥接的结果，但同样不能保证每个簇恰好对应一个语义物体。足够密集的桥仍然可能连接对象。
本例先移除背景平面，再使用 35 mm 邻域和最小 8 点得到两簇，中心分别约为：
text
簇 A 可见表面中心：(0.380589, 0.002848, 0.199998) m
簇 B 可见表面中心：(0.609908, −0.009790, 0.349904) m对应实现见 平面分割、欧式聚类与桥接反例。这里的簇大小不同，既受表面投影面积影响，也受深度、体素网格与采样位置影响，不能用"点多"直接推断物体更大或结果更可靠。
8. 点簇中心还不是抓取中心
相机通常只能看到物体朝向自己的一部分表面。点均值计算的是这些可见采样的中心，不是完整物体几何中心，更不是质量中心。视角变化、遮挡变化和降采样规则变化，都能移动这个均值，而物体实际上没有移动。
假设八界头部看见玩具车顶，腕部靠近后主要看见侧面。两次点簇中心不同，可能只是可见表面变了。若直接拿它们相减判断"目标移动"，会把观测变化误当成真实运动。跨视角关联需要结合外观、几何模型、坐标变换与时间证据。
点云 PCA 能估计主要延展方向，但也有边界：近球体的特征值相近时，主轴容易因一点噪声大幅转动；对称物体可能存在多个等价轴；有遮挡的局部表面主轴不一定对应物体设计坐标轴。后续第 17 篇讨论的六维位姿，需要明确物体参考系和对应关系，不能只把 PCA 三根轴包装成确定姿态。
规划需要的环境信息也与目标点集不同。移除地面可以帮助目标聚类，但地面和家具仍然是机械臂运动需要尊重的几何约束。应保留原始观测或环境表示，让"目标识别分支"与"碰撞检查分支"消费适合自己的数据。
9. 把头部点云与腕部点云合并前，先检查时间
两帧点都转换成名为 base_footprint 的坐标，并不保证可以直接叠加。移动底盘在不同时间的基座位置可能不同；手臂与升降状态也在变化。要比较静止环境，通常需要把各帧通过对应时刻的变换放到同一个约定的参考系，再判断场景是否真的静止。
一个简单量级：底盘以 0.3 m/s 移动，两帧相差 0.4 s，位移达到 0.12 m。若把两帧各自的局部基座坐标当成同一空间，玩具可能出现双影。这时增加体素尺寸只会把两个错误位置平均起来，并不能修复时间问题。
八界的头部定位、腕部复核流程因此适合保留两份独立观测，再显式进行跨视角匹配。图像匹配成功、坐标变换可查询、点云配准残差小，这些信息分别回答不同问题，不能互相替代。4
点云文件本身也可能丢失上下文。本篇可导出的 ASCII PLY 只写 XYZ 和米制说明，不包含完整相机标定、图像 ID 与采样戳。用于真实采集回放时，应该另存元数据；不要把一个能打开的 PLY 当成完整测量记录。
10. 运行示例与理解输出
在 AtomGit 仓库根目录执行：
bash
python3 -m venv .venv-vision
.venv-vision/bin/python -m pip install -r "03_视觉与三维感知/15_从深度图到点云/examples/requirements.txt"
.venv-vision/bin/python "03_视觉与三维感知/15_从深度图到点云/examples/15_pointcloud.py"最终输出 PASS: 8 experiments; synthetic data, no robot connection.。使用 --json 可读取完整结果；只有显式传入 --ply /tmp/bajie_objects.ply 才会写出点云文件。默认不创建输出目录。代码、依赖和预期输出 已按篇组织。
脚本为了把邻接关系写清楚，使用小规模 NumPy 两两距离计算，时间和内存都随点数平方增长。这不是处理几十万点的性能方案。迁移到真实高分辨率点云时，应使用 KD-tree、体素哈希或库中的近邻实现，同时保持距离单位、是否包含自身、边界比较和索引映射等语义一致。
理解这套流程以后，可以针对具体失败选择下一步：地面残留就检查平面模型与先验，远处目标破碎就检查密度与邻域尺度，两个玩具粘连就检查桥接和实例证据，中心跳动就检查可见表面变化。点云算法提供的是可检查的几何假设，而不是自动替八界定义物体和动作。
欢迎关注科沃斯八界 AtomGit，相关代码将逐步释放，实际开放内容与使用说明以仓库为准。5