体素、网格与 SDF

深度相机和激光雷达（10.3、12.1）给你的是点。可是机器人要用它们回答的几乎都是别的问题：

• 碰撞检测：机械臂在这个构型（7.2 节）下会不会碰到东西？需要一个"这个点在不在物体里面"的判断。
• 间距查询：离最近的障碍物还有多远？轨迹优化（6.13）的障碍代价靠它。
• 脱离方向：已经（快要）撞上了，朝哪个方向推最快脱离？这是一个方向，也就是梯度（3.2）。
• 地图融合：同一堵墙被看了一百次，每次都有噪声、遮挡，还有行人走过，怎样合成一张可信的图？这是概率（4.7）和平均。
• 表面与渲染：物体的外表长什么样？物理仿真、抓取规划、图形学（12.3）都要一张表面。

同一个形状可以用点、体素、网格、符号距离场、隐式函数存下来。每一种表示能便宜地回答一部分问题，另一部分则完全答不了。本节的目标是：看懂每种表示"擅长什么、代价是什么"，并学会它们之间怎样互相转换（体素 ↔ SDF ↔ 网格）。重点是 SDF——它是现代机器人避障、建图和神经隐式表示（12.4）共同的语言。

下面的形状是两个凸块相并、再挖掉一个洞。切换五种表示：点云（传感器直接给的，无序、有噪声、没有连接关系）；体素（把空间切成格子，每格一位"占据/空闲"）；网格（只存表面：顶点加连接，二维里是折线，三维里是三角形）；SDF（每个点存到表面的有向距离，颜色是数值，线是等值线）；隐式函数（零集相同，但数值不是距离，等值线疏密不均）。拖动分辨率：体素的细节随 \(n\) 提高，而三维里的内存按 \(r^3\) 增长，这是体素最大的痛点。

体素的账单。三维里每个方向 \(r\) 格，总共 \(r^3\) 格；分辨率翻倍，内存变成 8 倍：

可是物体只有表面是有信息的，表面的体素数只按 \(r^2\) 增长。所以实用系统都用稀疏结构：八叉树（OctoMap）、哈希体素块（Voxblox、nvblox 一类的建图库）、层级稀疏格（OpenVDB），只给表面附近分配内存。

机器人看到的世界不是一次给全的。占据栅格（Elfes, 1989）把平面切成格子，每格是一个二值随机变量"被占据 / 空闲"，并假设各格相互独立。一条测距射线给出两类证据：射线穿过的格子多半是空的；射线终止的那个格子多半被占据了；射线之外的格子，什么也没看见——仍然未知。把每一条证据用贝叶斯公式（4.7）并进去，在对数几率下就是简单的加法。

下图：一个 200° 视场的激光测距仪在房间里移动（房间里有一个盒子、一根柱子）。拖动机器人（琥珀色圆点）和朝向（蓝色小圆），沿途扫描并建图；或者按"自动巡航"。灰色 = 未知，青色 = 空闲，红色 = 占据，越深越确定。

占据栅格只回答"是 / 否"。符号距离场多存一点：到表面的距离，并用符号区分内外（本课约定里面为负）：

下面是你的第一个 SDF 实验室。选一种形状（圆 A、盒子 B、胶囊 AB，或它们的布尔组合），拖动 A、B 的中心和琥珀色探针。颜色是 \(d\) 的值（青 = 外，紫 = 内），细线是 \(d=\pm0.25,\pm0.5,\dots\) 的等值线，粗线是 \(d=0\)，也就是表面。探针处：琥珀色箭头是 \(\nabla d\)（外向法向，长度恒为 1），紫色虚线连向最近表面点 \(\mathbf x-d\nabla d\)，绿色虚线圆是以 \(\hl{ball}{|d|}\) 为半径的"安全球"：球内一定没有表面。探针外的圆盘是半径 \(\hl{rho}{\rho}\) 的机器人：当且仅当 \(d<\rho\) 时碰撞，红色虚线就是 \(d=\rho\) 的等值线，也就是机器人圆心不能越过的边界。

有了 SDF，怎样得到"看得见的表面"？有两条路。(a) 抽取：在网格上采样 \(d\)，找出符号变化的格子，用线性插值求出零点，连成折线（二维的 marching squares）或三角形（三维的 marching cubes，Lorensen & Cline 1987）。一个格子的 4 个角各有"里 / 外"两种状态，共 \(2^4=16\) 种情形（下方图例，编号是 4 位二进制）；三维是 \(2^8=256\) 种，按对称性归为 15 类，有些类有歧义（同一组符号对应不同连法），要靠额外规则或网格中心值来消歧；marching cubes 在尖锐的棱角处会被网格"磨圆"，dual contouring（Ju 等 2002）则每格放一个顶点，利用梯度信息保住棱角。(b) 直接追踪：不生成网格，沿相机射线向前走，每一步走 \(d(\mathbf x)\)：以 \(\mathbf x\) 为圆心、半径 \(d\) 的球里肯定没有表面，所以这一步绝对不会"跨过"表面（球追踪，Hart 1996）。

左图拖动网格大小，看折线怎样逼近真实轮廓（虚线）；把鼠标放在一个格子上，图例会高亮它属于哪一种情形。右图拖动射线的起点（绿）和方向点（琥珀），并改变场的缩放 \(s\)，让 \(f=s\,d\)：它不再是距离，而是"陡了 \(s\) 倍"的函数。

真实的传感器每次都有噪声，每次只看到一部分。截断符号距离函数（TSDF）融合（Curless & Levoy 1996；KinectFusion，Newcombe 等 2011 用 GPU 做到了实时）的做法极其朴素：让每个体素存一个 \(D\) 和一个权重 \(W\)。每一条射线测得距离 \(z\)，射线上深度为 \(t\) 的体素得到一个观测 \(\psi=\min(z-t,\ \mu)\)（表面前方为正，后方为负）；只更新离表面不太远的体素（\(z-t\ge-\mu\)，\(\mu\) 是截断距离）。然后把新观测并入加权平均：\(D\leftarrow\dfrac{WD+w\psi}{W+w}\)，\(W\leftarrow W+w\)。零等值面就是融合后的表面。

为什么平均能消除噪声：每一次观测的误差互相独立，平均 \(N\) 个以后，标准差按 \(1/\sqrt N\) 下降；而且这个平均是在场上做的，不需要先把不同视角的点一一对应起来（12.1 节的 ICP 里那个难点）。下图：一个深度传感器沿圆周从不同方向扫描同一个物体（一圈 60 条射线）。点"+1 次扫描"，对比原始点（噪声不变）与融合曲面（越来越准）；右图是沿竖线（可拖动）的一维切片：融合的 \(D\)（琥珀）对比真实的截断 SDF（虚线）。

设某格被占据的概率为 \(p\)，几率 \(\mathrm{odds}=p/(1-p)\)，对数几率 \(\ell=\ln\dfrac{p}{1-p}\)，逆变换 \(p=\dfrac1{1+e^{-\ell}}\)。给定测量 \(z\)，贝叶斯公式在几率形式下是"后验几率 = 先验几率 × 似然比"。各格独立、测量条件独立时：

每次测量只是加一个常数，所以更新是 \(O(1)\)，顺序无关，也不会数值下溢。钳位 \(\ell\in[\ell_{\min},\ell_{\max}]\) 让地图在环境变化后很快"改口"（FIG 2 的鬼影）。例如 \(p_{\rm hit}=0.7\)、\(p_{\rm miss}=0.4\)：\(\ell_{\rm hit}=+0.847\)，\(\ell_{\rm miss}=-0.405\)。OctoMap（Hornung 等 2013）的默认值正是 \(p_{\rm hit}=0.7\)、\(p_{\rm miss}=0.4\)，并把 \(\ell\) 钳在概率约 0.12 到 0.97 之间。

为什么 \(\|\nabla d\|=1\)。距离是 1-Lipschitz 的：\(|d(\mathbf x)-d(\mathbf y)|\le\|\mathbf x-\mathbf y\|\)（从 \(\mathbf x\) 走到 \(\mathbf y\)，到表面的距离最多变化走过的长度），所以 \(\|\nabla d\|\le1\)。另一方面，设 \(\mathbf c\) 是 \(\mathbf x\) 的最近表面点，沿线段从 \(\mathbf x\) 向 \(\mathbf c\) 走，距离恰好以单位速率减小，于是这个方向的方向导数是 \(-1\)。两者合起来：\(\nabla d\) 是单位向量，且指向远离 \(\mathbf c\) 的方向，即 \(\nabla d=(\mathbf x-\mathbf c)/d\)，推出最近点公式 \(\mathbf c=\mathbf x-d\nabla d\)。这就是 Eikonal 方程 \(\|\nabla d\|=1\)（几何光学里波前传播的方程）。它在中轴上不成立：那里有两个最近点，\(d\) 像山脊一样有折痕，梯度不存在（FIG 3 的紫色点）。

对变换友好。刚体变换 \((R,\mathbf t)\) 后的物体，其 SDF 为 \(d'(\mathbf x)=d(R^\top(\mathbf x-\mathbf t))\)，仍然是精确的 SDF（刚体运动保持距离）；整体放大 \(s\) 倍则是 \(d'(\mathbf x)=s\,d(\mathbf x/s)\)（只写 \(d(\mathbf x/s)\) 会让梯度模变成 \(1/s\)）。把物体向外"膨胀" \(r\)（与半径 \(r\) 的圆盘做 Minkowski 和）得到 \(d-r\)：因此圆盘（球）机器人与物体碰撞 \(\Leftrightarrow d(\mathbf c)<\rho\)，这就是 7.2 节"构型空间障碍物"的最简单一例。非圆形的机器人常用若干个球近似：\(\min_i\big(d(\mathbf c_i)-\rho_i\big)<0\) 即碰撞，而 \(\nabla d\) 还告诉你每个球该往哪里推。

布尔运算只是 min / max。并集 \(\min(d_A,d_B)\)，交集 \(\max(d_A,d_B)\)，差集 \(A\setminus B\) 是 \(\max(d_A,-d_B)\)。要诚实：它们只在一侧精确——并集的 \(\min\) 在外部等于真实距离，但在内部低估深度（一部分 A 的表面其实埋在 B 里）；交集的 \(\max\) 在内部精确、外部低估；差集在 B 的内部附近误差最大（FIG 3 的"差"）。好在它们仍然是 1-Lipschitz，球追踪依旧安全（只是保守）。要得到圆润的融合，常用多项式平滑最小值：\(\mathrm{smin}(a,b,k)=\min(a,b)-\dfrac{h^2k}{4}\)，\(h=\dfrac{\max(k-|a-b|,0)}{k}\)（这个形式由 Inigo Quilez 推广）。混合区里 \(\|\nabla\|<1\)，不再是严格的距离。

三角网格 = 顶点数组 \(\{\mathbf v_i\}\) + 面（三个顶点编号）。三角形 \((\mathbf v_0,\mathbf v_1,\mathbf v_2)\) 的法向和面积来自叉积（1.8）：\(\mathbf n=\dfrac{(\mathbf v_1-\mathbf v_0)\times(\mathbf v_2-\mathbf v_0)}{\|\cdots\|}\)，面积 \(=\tfrac12\|\cdots\|\)，顶点顺序（逆时针）决定法向朝哪边。水密（watertight）指每条边恰好被两个面共享（流形，manifold），没有洞和悬边；水密网格才有明确的内外（射线奇偶或绕数）。对亏格 0 的闭合网格，欧拉公式 \(V-E+F=2\)。网格到 SDF：到最近三角形的距离，符号取自最近特征的"伪法向"（Bærentzen & Aanæs 2005）。常用半边（half-edge）结构做邻接遍历。

\(f\) 在表面上为 0 且 Lipschitz 常数为 \(L\)，则 \(f(\mathbf x)\le L\,d(\mathbf x)\)，所以步长取 \(f/L\) 永远不会越过表面；真正的 SDF 有 \(L=1\)。FIG 4 里 \(f=s\,d\)，\(L=s\)：\(s>1\) 时不加 \(1/L\) 就会穿透，\(s<1\) 则安全但慢。步数：射线以掠射角 \(\alpha\) 靠近一个平面，每一步高度乘以 \((1-\sin\alpha)\)，\(h_0\) 是起始高度、\(\epsilon\) 是命中阈值，所以 \(\alpha\) 越小步数越多（在 node 里对 \(\alpha=0.05,0.1,0.3\) rad 验证过：预测 135、66、20 步，实测 136、67、21 次求值，后者多出的 1 次是最后那次命中判定）。

在格长 \(h\) 的网格上，对光滑、曲率为 \(\kappa\) 的表面，线性插值给出的顶点落在真实表面上的偏差量级是 \(O(\kappa h^2)\)（在 node 里对圆验证：\(n=8,16,32,64\) 时顶点处最大 \(|d|\) 实测为 \(2.3\times10^{-2}\)、\(6.9\times10^{-3}\)、\(1.9\times10^{-3}\)、\(5.1\times10^{-4}\)，每次翻倍约降 3.7 倍）。尖角处则不同：线性插值会把棱角削平，误差只按 \(O(h)\) 下降，FIG 4 的形状有尖角，所以读数里的误差降得慢一些。Dual contouring 在每个格里存一个顶点，位置取"各交点切平面到该点距离平方和最小"（一个小的二次误差函数 QEF）的解，因此能保住尖角。

所以更新规则是对一维最小二乘的递推解（1.13 节；4.4 节里两个高斯的融合是同一个加权平均）。权重 \(w\) 可以反映传感器的可信度（远处的深度更噪、斜入射更不可信）；KinectFusion 还把 \(W\) 封顶，使旧数据逐渐被遗忘，能适应变化。需要指出两点：(1) 沿射线的 \(z-t\) 是"射线方向的距离"，斜入射时比到表面的真实最短距离大，所以 TSDF 不是严格的欧氏距离，只在表面附近大致可用；(2) 截断以外的数值没有意义。

从 TSDF 到 ESDF。规划器要的是处处有效的欧氏 SDF（ESDF），而不是只有一层薄壳的 TSDF。做法：把 TSDF 的符号（或权重阈值）当作占据 / 空闲标记，再做距离变换（精确的欧氏距离变换可以在每个维度上 \(O(N)\) 完成，Felzenszwalb & Huttenlocher 2012）。机器人建图的库一般做增量版本：只传播被新数据改变的体素（Voxblox、FIESTA 的思路）。

用神经网络 \(f_\theta(\mathbf x)\) 代替网格上的数值：DeepSDF（Park 等 2019）学一个"形状码 + 坐标 → 符号距离"的网络；Occupancy Networks（Mescheder 等 2019）输出占据概率；IGR（Gropp 等 2020）把 Eikonal 条件写成损失 \(\mathbb E(\|\nabla f_\theta\|-1)^2\) 来逼网络成为 SDF；NeuS、VolSDF（2021）把 SDF 放进体渲染，从多视图图像恢复表面。它们的优势是连续、紧凑、可微，代价是查询要跑网络、不能直接编辑。12.4 节会再用到这些思想。

SDF 在机器人里最直接的用途是避障代价。回忆 6.13 节：把轨迹看成一串路点 \(\mathbf q_1,\dots,\mathbf q_n\)，代价 = 平滑项 + 障碍项。障碍项在路点离障碍的距离 \(d\) 小于安全边距 \(\varepsilon\) 时罚 \(c(d)=\tfrac12(\varepsilon-d)^2\)，它的负梯度 \(-\nabla c=(\varepsilon-d)\nabla d\) 把路点沿 SDF 梯度推离障碍。6.13 里障碍是一个解析的圆盘，\(d\) 随手可写；而真实的障碍是点云、网格、或一张融合出来的 TSDF——于是你需要一张查询表：把障碍预先烘焙成栅格上的 ESDF，运行时对栅格做双线性插值（得到 \(d\)），再对插值函数求梯度（得到 \(\nabla d\)）。这正是 CHOMP（Ratliff 等 2009）以及后来许多基于栅格 ESDF 的规划器（如 Voxblox、FIESTA 之上的无人机规划）的做法。

下图复现这个流程：拖动红色圆圈（障碍中心）、绿色圆圈（起点 / 终点）。按"优化"，路点沿 \(-\nabla F\)（平滑项 + 障碍项）下降；红色小箭头是障碍项的推力。把"网格 ESDF"换成"解析 SDF"、把栅格边长调大，观察栅格误差怎样影响结果。