BVH 与雷达点云表面：如何在一个已重建的表面上插入一个点位

在雷达点云重建出的表面上"插入一个点位"，听起来是一个几何动作：找一个位置，放一个点。

但把它放进一条真实的处理流水线里，问题会变成三个：插在哪里（定位）、该不该插（一致性校验）、插完以后加速结构还对不对（更新）。BVH（Bounding Volume Hierarchy，层次包围盒）在这三步里都会出现，而最后一步才是真正容易出事的地方。

先给结论：

BVH 负责把"在几百万个三角形里找落点"从线性扫描降到对数级；但插入本身是一个局部拓扑操作，它的难点不在插入，而在于插入之后加速结构是否仍然有效、以及雷达量测的各向异性误差是否被正确传递。

本文所有代码片段都来自一份约 380 行、只依赖 Python 标准库的参考实现，并且已经在本机跑通（第八节是真实输出）。为了控制篇幅，正文按主题摘录关键部分，省略了向量辅助函数、Node / BVH 的样板代码，以及构建函数中的分支细节。所有实测数字都标注了运行环境：纯 Python 的绝对值不能外推到生产系统，但比值有参考意义。

一、先定义问题：什么叫"插入一个点位"

这个说法至少对应三种不同的操作，混在一起讨论必然出错。

三者的共同点是：都需要先做一次空间查询，再做一次局部拓扑修改。区别在于查询用哪一种、以及"落点"要不要被量测拉动。

本文以 S3 为主线，因为它是雷达场景下唯一有实际意义的用法——单帧雷达点云根本不足以构成表面。S3 的定位既可以走投影（复用 S1 的查询），也可以走射线（复用 S2 的查询），本文用投影。

二、雷达点云表面为什么特殊

如果表面来自激光雷达或深度相机，插入点位基本就是一个纯几何问题。雷达不行，原因是可量化的。

2.1 分辨率在两个方向上差了十几倍

距离分辨率只由带宽决定：

$$\Delta R = \frac{c}{2B}$$

1 GHz 带宽对应 15 cm，而且与距离无关。实测数据也在这个量级：某 77 GHz 雷达标称 $\Delta r = 0.15\ \text{m}$¹。

横向分辨率则完全是另一回事。77 GHz 对应波长 $\lambda = c/f \approx 3.90\ \text{mm}$，实孔径雷达的横向分辨单元是

$$\delta_{cr} \approx R \cdot \theta_{3dB} \approx R \cdot \frac{k\lambda}{D}, \quad k \approx 0.886$$

它随距离线性变差。用真实数字感受一下²：

• 77 GHz、约 47 mm 接收孔径 → 方位波束约 5° → 50 m 处横向分辨单元 4.4 m；
• 把 MIMO 虚拟阵列扩到 100+ 通道，波束压到 1.5° 以内 → 50 m 处约 1.3 m；
• 量产 4D 成像雷达的标称值普遍在方位 1°–2°、俯仰 2° 上下³。

也就是说，在 50 m 处，雷达的一个"像素"大约是 0.15 m × 1 m——一个被极度拉长的椭圆。

graph TD
    A["雷达在 50 m 处的一个分辨单元"] --> B["距离方向：0.15 m<br/>由带宽决定，与距离无关"]
    A --> C["横向：约 1.0 m<br/>由 λ/D 决定，随距离线性变差"]
    B --> D["结果：各向异性采样"]
    C --> D
    D --> E["单帧点云稀疏且分布不均"]
    E --> F["必须多帧累积才能重建表面"]

2.2 单帧点云重建不出表面

这不是工程实现问题，而是信息量问题。4D 成像雷达在室内场景单帧输出约 3,000–6,000 点，但分摊到每个行人身上每帧只有几个点，而且雷达对地面的回波很弱⁴。不同厂商的滤波策略差异极大：同一台车上的双 4D 雷达对比测试中，Arbe Phoenix 每帧 6K–14K 点，而 ARS548 RDI 只有 400–650 点，同车激光雷达是 110K–120K³。

所以工程上的标准做法是多帧累积：用里程计得到 $\mathrm{SE}(3)$ 位姿并插值到雷达时间戳，再做

$$P_{acc} = P_t \cup \left(T \cdot P_{t-1}\right)$$

常见窗口是 3–6 帧，也有只用 2 帧的方案⁴。需要说明的是，针对"表面重建"这个任务，累积多少帧最优并没有公认结论——我没能找到任何给出定量最优值的文献，参考文献里给的都是各自任务下的经验取值。

2.3 误差是各向异性的，而且横向误差会"藏起来"

这一点最关键。雷达的距离精度显著优于横向精度，后者等于角度精度乘以距离⁵：

$$\sigma_{cross} = R \cdot \sigma_\theta$$

取 $\sigma_R = 5\ \text{cm}$、$\sigma_\theta = 1° = 0.0175\ \text{rad}$，在 20 m 处：

差了 7 倍。 而这个差异在"点到表面"的残差里几乎看不见——因为横向误差是沿着表面滑动的，它不产生离面残差。

后面第六节会用实测数字说明这件事的后果：一次量测可以完美通过一致性检验，然后被插到离真值 35 cm 的位置上。

2.4 还有鬼影

毫米波波长远大于激光，场景中大量表面相对波长而言是"平坦"的，于是产生镜面反射和多径，形成鬼影目标¹。在一个包含 3500 万雷达点的数据集里，约 100 万个标注中有约 10 万个是鬼影点¹。此外还有栅瓣角度模糊、TDM-MIMO 带来的多普勒模糊、以及 RCS 起伏。

插入流程必须带一个门限——否则鬼影会被当作真实表面几何写进地图，而且写进去就拿不出来了。

三、BVH：一个为空间查询而生的层次结构

BVH 是一棵二叉树，每个节点持有一个轴对齐包围盒（AABB），根节点包住全部几何，叶节点持有若干图元（这里是三角形）。查询时，如果射线或查询点与某个节点的 AABB 都不相交/距离过远，整个子树就被剪掉。

对于"插入点位"这个任务，BVH 相对其他结构的优势很具体：

选 BVH 的核心理由是：射线求交和最近点查询可以共用同一棵树，而 S1/S2/S3 三种插入语义恰好各需要其中一种。

四、构建：分桶 SAH

构建的目标是让"期望查询代价"最小。SAH（Surface Area Heuristic，表面积启发式）的思路是：一个 AABB 被随机方向的射线击中的概率正比于它的表面积，所以把图元划分成左右两组 $L, R$ 的代价可以写成

$$C = C_{trav} + \frac{A_L}{A_P} N_L C_{isect} + \frac{A_R}{A_P} N_R C_{isect}$$

其中 $A_P$ 是父节点表面积，$N_L, N_R$ 是两边的图元数。取 $C_{trav} = C_{isect} = 1$，判断"继续划分"是否划算就变成比较

$$A_P + A_L N_L + A_R N_R \quad \text{vs} \quad A_P N$$

朴素 SAH 需要对每个候选切分点求一次代价，而候选点有 $O(N)$ 个，直接做会让构建退化到 $O(N \log^2 N)$ 甚至更差。分桶（binning）是标准优化：把图元按质心在切分轴上的位置装进 $B$ 个桶（实践中 $B = 12 \sim 32$），只用前缀/后缀扫描算出每个桶边界的左右 AABB 与计数，然后一次线性扫描评估 $B-1$ 个候选切面。

这样每层的划分是 $O(N)$，总构建代价回到 $O(N \log N)$。代价是 SAH 的近似——但实验表明分桶 SAH 的树质量与精确 SAH 差距很小，而构建快得多⁶⁷。

先给出后面反复用到的向量辅助函数和 AABB：

def sub(a, b): return (a[0] - b[0], a[1] - b[1], a[2] - b[2])
def add(a, b): return (a[0] + b[0], a[1] + b[1], a[2] + b[2])
def mul(a, s): return (a[0] * s, a[1] * s, a[2] * s)
def dot(a, b): return a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
def cross(a, b):
    return (a[1] * b[2] - a[2] * b[1],
            a[2] * b[0] - a[0] * b[2],
            a[0] * b[1] - a[1] * b[0])
def length(a): return math.sqrt(dot(a, a))
def unit(a):
    n = length(a)
    return (a[0] / n, a[1] / n, a[2] / n) if n else (0.0, 0.0, 0.0)


class AABB:
    __slots__ = ("lo", "hi")

    def __init__(self, lo=(INF,) * 3, hi=(-INF,) * 3):
        self.lo, self.hi = lo, hi

    def is_empty(self):
        return self.lo[0] > self.hi[0]

    def expand_point(self, p):
        self.lo = tuple(min(self.lo[i], p[i]) for i in range(3))
        self.hi = tuple(max(self.hi[i], p[i]) for i in range(3))

    def expand_box(self, b):
        if b.is_empty():
            return
        self.lo = tuple(min(self.lo[i], b.lo[i]) for i in range(3))
        self.hi = tuple(max(self.hi[i], b.hi[i]) for i in range(3))

    def area(self):
        if self.is_empty():
            return 0.0
        d = (self.hi[0] - self.lo[0], self.hi[1] - self.lo[1], self.hi[2] - self.lo[2])
        return 2.0 * (d[0] * d[1] + d[1] * d[2] + d[2] * d[0])

    def center(self):
        return tuple(0.5 * (self.lo[i] + self.hi[i]) for i in range(3))

    def sqdist(self, p):
        """点到 AABB 的最短距离平方——这是最近点查询的剪枝下界。"""
        s = 0.0
        for i in range(3):
            d = self.lo[i] - p[i] if p[i] < self.lo[i] else (
                p[i] - self.hi[i] if p[i] > self.hi[i] else 0.0)
            s += d * d
        return s

分桶 SAH 的核心循环：

axis = max(range(3), key=lambda i: cb.hi[i] - cb.lo[i])   # 质心跨度最大的轴
span = cb.hi[axis] - cb.lo[axis]
if span < 1e-12:                     # 质心全部重合，SAH 无法分辨
    return self._leaf(node, items)
lo, B = cb.lo[axis], self.bins
scale = B / span
counts, boxes = [0] * B, [AABB() for _ in range(B)]
for f in items:                      # 第一遍：装桶，O(N)
    b = min(B - 1, int((self.centroids[f][axis] - lo) * scale))
    counts[b] += 1
    boxes[b].expand_box(self.fboxes[f])

# 前缀 / 后缀：一次性得到每个桶边界左右两侧的 AABB 与计数
left_box, left_cnt, acc = [None] * B, [0] * B, AABB()
c = 0
for i in range(B):
    c += counts[i]; left_cnt[i] = c; acc.expand_box(boxes[i])
    left_box[i] = AABB(acc.lo, acc.hi)
right_box, right_cnt, acc = [None] * B, [0] * B, AABB()
c = 0
for i in range(B - 1, -1, -1):
    c += counts[i]; right_cnt[i] = c; acc.expand_box(boxes[i])
    right_box[i] = AABB(acc.lo, acc.hi)

# 第二遍：评估 B-1 个候选切面，与"就此停止"比较
area, best, best_cost = node.box.area(), -1, node.box.area() * len(items)
for i in range(B - 1):
    if left_cnt[i] == 0 or right_cnt[i + 1] == 0:
        continue
    cost = area + left_box[i].area() * left_cnt[i] + right_box[i + 1].area() * right_cnt[i + 1]
    if cost < best_cost:
        best_cost, best = cost, i

这里有两个容易踩的坑：质心跨度为零时 SAH 失效（所有质心重合，任何切分都不减少重叠），此时应回退到中位数切分；以及必须设置叶子容量上限（max_leaf），否则退化几何会导致树无限加深。

五、查询：从图元原语到两类遍历

插入流程需要两种空间查询。它们的底层是同一组图元原语，但遍历策略不同。

5.1 两个图元原语

先看最底层的两块砖：它们都在三角形局部完成，不需要任何空间结构。

射线—三角形求交用 Möller–Trumbore：它把"与三角形平面求交"和"判断交点是否落在三角形内"两步合并，不必预先求平面方程。

def ray_triangle(o, d, a, b, c):
    """Möller–Trumbore，返回 (t, 重心坐标) 或 None；d 需已归一化。"""
    e1, e2 = sub(b, a), sub(c, a)
    pv = cross(d, e2)
    det = dot(e1, pv)
    if abs(det) < 1e-14:
        return None                                     # 射线平行于三角形平面
    inv = 1.0 / det
    tv = sub(o, a)
    u = dot(tv, pv) * inv
    if u < 0.0 or u > 1.0:
        return None
    qv = cross(tv, e1)
    v = dot(d, qv) * inv
    if v < 0.0 or u + v > 1.0:
        return None
    t = dot(e2, qv) * inv
    return (t, (1.0 - u - v, u, v)) if t > 1e-9 else None

三角形上最近点的计算用 Ericson《Real-Time Collision Detection》§5.1.5 的区域判定写法⁸：把平面划成 7 个区域（3 个顶点、3 条边、内部），用点积的符号判断查询点落在哪里，一次算出最近点和重心坐标，避开逐条边求最近点的循环。它返回的重心坐标正好是插入时算新顶点位置要用的东西——所以"定位"和"落点"可以复用同一次计算，不需要额外查询。

这里有个容易低估的实现成本：这个函数的边界情况比看上去多——零面积三角形、查询点落在区域分界线上、浮点比较容差。生产实现通常在构建阶段就把退化三角形剔除掉。

5.2 射线求交：有序遍历 + 早退

先做射线/AABB 的平板（slab）测试。用预计算的倒数方向避免每次除法，同时把 0 * inf = nan 的问题交给 IEEE 的 ±inf 语义处理：

def slab(o, inv_d, box, t_max):
    """射线/AABB 平板测试，返回进入参数 t_enter 或 None。"""
    t0, t1 = 0.0, t_max
    for i in range(3):
        lo = (box.lo[i] - o[i]) * inv_d[i]
        hi = (box.hi[i] - o[i]) * inv_d[i]
        if lo > hi:
            lo, hi = hi, lo
        if lo > t0:
            t0 = lo
        if hi < t1:
            t1 = hi
        if t0 > t1:
            return None           # 该轴上的区间为空 -> 不相交
    return t0

遍历时维护"当前最近命中距离" best_t，它同时充当剪枝阈值。先访问更近的孩子，可以让更远的那个尽早被 best_t 剪掉：

while stack:
    node = stack.pop()
    if slab(origin, inv, node.box, best_t) is None:
        continue                      # 整个子树被剪掉
    if node.is_leaf:
        for f in node.faces:
            i, j, k = self.faces[f]
            hit = ray_triangle(origin, d, self.verts[i], self.verts[j], self.verts[k])
            if hit and hit[0] < best_t:
                best_t, best = hit[0], (f, hit[0], hit[1])
        continue
    tl = slab(origin, inv, node.left.box, best_t)
    tr = slab(origin, inv, node.right.box, best_t)
    if tl is not None and tr is not None:      # 近的后入栈 -> 先出栈
        near, far = (node.left, node.right) if tl <= tr else (node.right, node.left)
        stack.append(far); stack.append(near)
    elif tl is not None:
        stack.append(node.left)
    elif tr is not None:
        stack.append(node.right)

一个生产环境的坑：浮点误差会让本应命中的射线"漏掉"包围盒，表现为随机的空洞。标准做法是在构建时给叶节点包围盒加一点 padding，或者在遍历时给 $t_{max}$ 留几个 ulp 的余量⁹。

5.3 最近点查询：分支限界

射线求交是"沿一条线"，最近点是"围绕一个点"，所以下界要换成点到 AABB 的最短距离（AABB.sqdist）。这就是分支限界：如果某个节点的下界已经不小于当前最优解，整个子树可以剪掉。

best, best_face, best_hit = INF, None, None
stack = [(self.root, self.root.box.sqdist(q))]
while stack:
    node, d2 = stack.pop()
    if d2 >= best:
        continue                      # 下界已不可能更优
    if node.is_leaf:
        for f in node.faces:
            i, j, k = self.faces[f]
            p, bary = closest_point_on_triangle(q, self.verts[i], self.verts[j], self.verts[k])
            dd = sub(q, p)
            dist2 = dot(dd, dd)
            if dist2 < best:
                best, best_face, best_hit = dist2, f, (p, bary)
        continue
    cl, cr = node.left.box.sqdist(q), node.right.box.sqdist(q)
    order = ((node.left, cl), (node.right, cr)) if cl < cr else ((node.right, cr), (node.left, cl))
    for child, cd2 in reversed(order):     # 近的先出栈
        if cd2 < best:
            stack.append((child, cd2))

5.4 复杂度

“期望 $O(\log N)$“里的期望是针对典型场景的；当查询点落在密集区域、或几何高度重叠时，剪枝会失效退化成线性扫描。

六、插入的完整流程

flowchart TD
    A["一帧雷达量测 z"] --> B["多帧累积 + 自车运动补偿"]
    B --> C["三角网格表面<br/>Poisson / BPA / MLS"]
    C --> D["构建 BVH"]
    D --> E["查询：定位落点"]
    E -->|射线| F["ray_cast -> 面 f + t + 重心坐标"]
    E -->|投影| G["closest_point -> 面 f + 重心坐标"]
    F --> H["各向异性一致性门限"]
    G --> H
    H -->|超限| I["拒绝：疑似鬼影 / 动态物体"]
    H -->|通过| J["1→3 局部细分"]
    J --> K["更新叶内三角形列表<br/>+ 反向索引 face_to_leaf"]
    K --> L{"包围盒变大了吗？"}
    L -->|否：点在原三角形内| M["无需 refit"]
    L -->|是：沿法向偏移| N["自底向上 refit 祖先链"]
    M --> O["完成"]
    N --> O

6.1 定位

对 S1/S3，落点是 $z$ 在表面上的最近点，同时返回重心坐标 $(\lambda_1, \lambda_2, \lambda_3)$——后面插顶点要用它算坐标。

def locate_by_projection(self, z):
    d2, f, hit = self.bvh.closest_point(z)
    if f is None:
        return None
    p, bary = hit
    return {"face": f, "distance": math.sqrt(d2), "bary": bary, "point": p}

6.2 门限：必须按各向异性拆开算

这是整篇文章里最容易被做错的一步。

直觉做法是算欧氏距离然后和某个 $\sigma$ 比较。但雷达的 $\sigma_R$ 和 $\sigma_{cross}$ 差了 7 倍（第 2.3 节），用单一 $\sigma$ 必然误判。正确做法是把残差 $\mathbf{r} = z - p^*$ 分解到量测自身的坐标系：沿视线的 1 个径向分量用 $\sigma_R$ 衡量，垂直于视线的 2 个横向分量各自用 $R\sigma_\theta$ 衡量：

$$\chi^2 = \left(\frac{r_\parallel}{\sigma_R}\right)^2 + \frac{r_{\perp,1}^2}{\left(R,\sigma_{\theta,az}\right)^2} + \frac{r_{\perp,2}^2}{\left(R,\sigma_{\theta,el}\right)^2}$$

三个分量相互独立，所以 $\chi^2$ 服从 3 自由度的卡方分布，取 99% 分位约 11.34 作为门限。（若偷懒把两个横向分量合成一个，就变成 2 自由度、门限 9.21——数字会有细微差别，但下面的结论不变。）

def gate(self, z, hit, sigma_range, sigma_angle_az, sigma_angle_el=None):
    """各向异性一致性检验：残差拆成径向 1 维 + 横向 2 维，各用自己的 σ。"""
    sigma_angle_el = sigma_angle_az if sigma_angle_el is None else sigma_angle_el
    u = unit(sub(z, self.origin))
    R = length(sub(z, self.origin))
    r = sub(z, hit["point"])
    r_par = dot(r, u)                               # 径向分量（1 维）
    r_vec = sub(r, mul(u, r_par))                   # 横向分量（垂直于视线的平面内）
    t1 = cross(u, (0.0, 0.0, 1.0))
    if length(t1) < 1e-9:                           # 视线与 z 轴平行时换一组基
        t1 = cross(u, (0.0, 1.0, 0.0))
    t1 = unit(t1)
    t2 = cross(u, t1)
    r_t1, r_t2 = dot(r_vec, t1), dot(r_vec, t2)
    s_az, s_el = R * sigma_angle_az, R * sigma_angle_el
    m2 = (r_par / sigma_range) ** 2 + (r_t1 / s_az) ** 2 + (r_t2 / s_el) ** 2
    return {"mahalanobis2": m2, "radial": r_par,
            "tangential": math.sqrt(r_t1 ** 2 + r_t2 ** 2),
            "sigma_cross": s_az, "range": R}

6.3 拓扑：1→3 细分

落点确定后，把所在三角形绕新顶点一分为三。这是增量 Delaunay 三角化里"插入顶点"的最基础形式；如果需要恢复 Delaunay 性质，还要在邻域做边翻转（Lawson flip），因为在三角形内部插入一个点会破坏空外接圆性质¹⁰。

def insert_on_face(self, f, bary, offset=0.0, normal=None):
    """把三角形 f 绕新顶点一分为三（1→3 细分），返回新顶点下标。"""
    i, j, k = self.faces[f]
    a, b, c = self.verts[i], self.verts[j], self.verts[k]
    p = add(add(mul(a, bary[0]), mul(b, bary[1])), mul(c, bary[2]))
    if offset:                                      # 沿法向抬升（点离开原平面）
        n = normal if normal is not None else cross(sub(b, a), sub(c, a))
        ln = length(n)
        if ln > 0.0:
            p = add(p, mul(mul(n, 1.0 / ln), offset))
    nv = len(self.verts)
    self.verts.append(p)
    self.faces[f] = [i, j, nv]                      # 原三角形就地复用，下标不变
    f2, f3 = len(self.faces), len(self.faces) + 1
    self.faces.append([j, k, nv])
    self.faces.append([k, i, nv])
    for nf in (f, f2, f3):
        box = self._face_box(nf)
        if nf < len(self.fboxes):
            self.fboxes[nf], self.centroids[nf] = box, box.center()
        else:
            self.fboxes.append(box)
            self.centroids.append(box.center())
    leaf = self.face_to_leaf[f]
    leaf.faces.extend((f2, f3))                     # 新三角形仍落在原叶内
    self.face_to_leaf[f2] = self.face_to_leaf[f3] = leaf
    if offset:                                      # 只有几何变大时才 refit
        self.refit_from(leaf)
    return nv

这段代码里有两个不显然的设计，都值得单独说。

6.4 不变量：落在三角形内部的点，不会扩大任何包围盒

三个子三角形的并集恰好等于原来的三角形。所以：

• 原来持有该三角形的叶节点，其 AABB 仍然完全有效；
• 所有祖先节点的 AABB 也仍然有效；
• 一次 refit 都不需要做。

这不是近似，是严格成立的。它意味着"在表面上插入一个点"的加速结构更新代价是 $O(1)$（只需要更新叶内的三角形列表和反向索引），而不是通常以为的 $O(\log N)$。

一旦落点带法向偏移（offset != 0），几何就真的变大了，这时才需要沿父指针自底向上 refit：

def refit_from(self, leaf):
    """自底向上重算祖先 AABB。只在包围盒真的变大时才需要。"""
    node = leaf
    while node is not None:
        box = AABB()
        if node.is_leaf:
            for f in node.faces:
                box.expand_box(self.fboxes[f])
        else:
            box.expand_box(node.left.box)
            box.expand_box(node.right.box)
        node.box = box
        node = node.parent

6.5 反向索引：face_to_leaf

insert_on_face 里有一行 leaf = self.face_to_leaf[f]。没有这个索引，插入时就不知道新三角形该挂到哪个叶上，只能全树搜索——反向索引是让插入保持 $O(1)$ 的关键，不是可选优化。

七、代价会累积：插入对 BVH 质量的影响（实测）

第六节证明了单次插入很便宜。但便宜不等于免费——叶节点的三角形列表会无限增长，而树结构不会因为插入而重新平衡。

我做了对照实验来量化这件事。场景是一面 12 m × 12 m 的墙（24×24 网格，1152 个三角形），沿 4000 个随机方向做射线定位并插入（约 3300 次命中，网格增长到 7768 个三角形），然后用 200 次最近点查询测性能。全部为纯 Python 实现。

怎么读这张表：

1. 不能拿 3983 和 2444 直接比。 插入后几何量本身变成了 7768 个三角形，SAH 代价自然上升。正确的对照是"同一份几何、重新建一棵树”——也就是 C 策略的结果 3983。
2. 所以真正的退化倍率是 $6771 / 3983 \approx 1.70$：只插入不维护的树，比同几何新建的树差 1.7 倍；查询慢 $41.8 / 23.2 \approx 1.8$ 倍。
3. 叶分裂（B）能挽回一部分，代价可控（阈值 16 时 382 次分裂共 39.8 ms），但它只能修局部，无法修复整棵树的划分质量。
4. 全量重建（C）质量最好，但在这份纯 Python 实现里重建耗时压倒性（3 次重建 1084 ms）。这个比例不能外推——生产级多线程构建器（Embree 等）在 $10^6$ 量级三角形上通常只需几十到几百毫秒，重建/插入的代价比远小于此。

工程结论是两条：

• 批量插入 + 周期性重建是默认策略。雷达本来就是按帧来的，与其逐个点插入，不如累积一帧后在帧间隙重建，顺便把新点的位置一次性整理好。
• 叶分裂作为重建之间的兜底：给每个叶设一个容量上限，超了就沿质心跨度最大的轴二分。这能防止单个叶无限膨胀。

需要强调的是，这些绝对数字来自纯 Python 玩具实现。有意义的只有比值（1.7 倍退化、1.8 倍查询变慢）和趋势（不维护会持续变差）。生产环境的数字完全不同。

八、跑一遍：实测输出

把前面各节的代码拼起来，对着 4608 个三角形的墙面网格跑一次完整的插入流程：

网格：4608 个三角形 / 2401 个顶点
最近点查询 vs 暴力法，最大偏差 0.00e+00

[正常量测] 接受=True  径向残差=+0.039 m  横向残差=0.007 m
           卡方=0.62 (门限 11.34)  距离 20.4 m 处 σ_横向=0.357 m
           插入顶点 2401 于 (20.0, 3.35, 1.5)，而真值在 (20.0, 3.0, 1.5)
           -> 切向错位 0.350 m

[鬼影量测] 接受=False  理由=超出门限（疑似鬼影）  径向残差=+3.962 m  卡方=6279.1

插入后遍历全树：每个节点的 AABB 仍包含其后代 —— 未做任何 refit
新顶点可被最近点查询找回，距离 0.00e+00 m

这段输出里有三件事值得注意。

第一，查询是正确的。 最近点结果与暴力法逐一比对，200 次查询最大偏差为 0。

第二，鬼影被正确拒绝了。 一个位于墙后 4 m 的多径鬼影，径向残差 3.96 m，$\chi^2 = 6279$，远超门限 11.34。如果它通过，这块"墙"就会被写上一个根本不存在的几何结构。

第三，也是最值得警惕的：正常量测通过了检验，但插错了 35 cm。

看那行 径向残差=+0.039 m——4 cm，在 $\sigma_R = 5$ cm 以内；横向离面残差 0.007 m。从"点到表面"的角度看，这个量测好得不能再好，卡方只有 0.62。

但插入的顶点在 $y = 3.35$，真值在 $y = 3.0$——切向错位 0.350 m，正好等于该距离处的 $\sigma_{cross} = R\sigma_\theta = 20.4 \times 0.0175 = 0.357\ \text{m}$。

原因在 2.3 节已经埋下：横向误差是沿着表面滑动的，它几乎不产生离面残差，所以一致性检验看不见它。这个问题在设计插入流程时必须显式处理：

• 把切向协方差跟着点一起存。插入的不是一个坐标，而是一个"位置 + 各向异性协方差”。
• 不要用各向同性的核做平滑。在 20 m 处，切向上相隔 35 cm 的两个点其实落在同一个分辨单元里，但各向同性核会把它们当成"远处的点"而降低权重——恰好把雷达唯一有效的方向（径向）的先验也一起抹掉了。
• 如果要让量测拉动表面，用带权更新：$p_{new} = p^* + W(z - p^*)$，其中 $W = \Sigma_{surf}(\Sigma_{surf} + \Sigma_{meas})^{-1}$，而不是直接把 $z$ 放上去。

还要注意，这个例子里表面恰好垂直于视线，所以横向误差只造成切向错位。如果表面是倾斜的或弯曲的（护栏、车辆侧面），横向误差会按法向投影出一部分离面残差——那时门限才会对它有所反应，但反应是"误杀"：一个正常的量测会因为表面倾斜而被判为离群。这也是各向异性门限必须用 $\sigma_{cross}$ 而不是单一 $\sigma$ 的原因。

九、雷达场景的四个坑

还有一条容易被忽略：雷达对地面回波很弱⁴。如果你的表面重建算法默认输入包含地面，雷达点云会让它重建出一张不存在的"地板"。这不是 BVH 的问题，但会让插入的点位落到错误的地方。

十、工程选型：不要自己写

本文的代码是为了讲清楚原理。生产环境请直接用成熟的实现：

用 Open3D 做原型时，插入流程大致是：

import open3d as o3d

scene = o3d.t.geometry.RaycastingScene()
scene.add_triangles(mesh)                       # 构建加速结构

# 定位：最近点
ans = scene.compute_closest_points(query_points)
# -> points / geometry_ids / primitive_ids

# 定位：射线
rays = o3d.core.Tensor([[ox, oy, oz, dx, dy, dz]], dtype=o3d.core.Dtype.Float32)
hits = scene.cast_rays(rays)
# -> t_hit / primitive_ids / primitive_normals

拿到 primitive_ids 之后，拓扑修改仍然要自己做（Open3D 不提供增量网格编辑），但至少空间查询这一层不需要重新发明。

几个实现层面的坑

• 浮点鲁棒性：遍历时的假阴性会表现为随机空洞，需要给包围盒留 padding⁹。
• 退化三角形：零面积三角形会让法向和重心坐标计算全部失效，构建时应剔除或单独处理。
• 空叶与空根：所有几何被删除或全部退化时必须能优雅返回 None，而不是崩溃。
• 并发：典型负载是"每帧大量查询 + 少量写入"。读多写少时用双缓冲或 RCU 切换整棵树，比给 BVH 加锁简单得多。
• 内存布局：生产级实现会把节点打包成定长结构（如 8 个 float 表示两个子盒）以配合 SIMD，和本文的 Python 对象模型完全不同。

总结

把"在雷达点云表面上插入一个点位"拆开看：

插入 = 空间查询（BVH） + 一致性校验（各向异性门限）
     + 局部拓扑修改（1→3 细分） + 加速结构维护（叶更新 / 定期重建）

三条最值得记住的结论：

1. BVH 让定位从 $O(N)$ 降到期望 $O(\log N)$，这是它能用在这条流水线里的唯一原因。 4608 个三角形的墙面上，纯 Python 实现的最近点查询平均 20.7 µs，暴力遍历是 9168 µs——约 440 倍。生产级实现因为 SIMD 和多线程，差距还会更大。

2. 落在三角形内部的插入不需要 refit。 1→3 细分的三个子三角形恰好覆盖原三角形，所有祖先的 AABB 依然有效。这是严格的不变量，不是近似。

3. 雷达的插入难点不在几何，在误差传递。 一次通过全部一致性检验的量测，可以在切向上错位 35 cm——因为横向误差沿着表面滑动，离面残差根本看不见它。如果插入流程里没有显式携带各向异性协方差，那么插进去的点位精度就是不可控的。

最后，别忘了维护策略：只插入不重建的 BVH，实测会比同几何新建的树差约 1.7 倍，查询慢约 1.8 倍，并且会持续恶化。批量插入 + 周期性重建 + 叶分裂兜底是稳妥的默认选择。

参考资料

1. Physically Based Rendering: Bounding Volume Hierarchies — BVH 构建与 SAH 的经典教材章节
2. How to build a BVH — Part 1: Basics / Part 2: Faster Rays — 从零实现 BVH 与 SAH 的实践教程
3. Robust BVH Ray Traversal — 浮点鲁棒性问题的标准处理
4. Robust Iterative Find-Next-Hit Ray Traversal — 用优先队列做前向遍历
5. Bittner, Hapala, Havran. Fast Insertion-Based Optimization of Bounding Volume Hierarchies, Computer Graphics Forum 32(1), 2013；及 Incremental BVH construction for ray tracing, Computers & Graphics 47, 2015 — 增量维护 BVH 质量
6. Ericson. Real-Time Collision Detection, §5.1.5 — 三角形最近点与重心坐标
7. Open3D RaycastingScene / Distance Queries 教程
8. Embree — 生产级 CPU 光线求交内核
9. CGAL AABB Tree
10. Cross-Range Resolution — $\delta_{cr} \approx R\theta_{3dB}$ 与 77 GHz 实例
11. Range Resolution — $\Delta R = c/2B$
12. Kraus et al. The Radar Ghost Dataset, IROS 2021 — arXiv:2404.01437
13. Dual 4D radar 与激光雷达对比评测
14. 4D 成像雷达点云特性与多帧累积
15. Shi et al. Moving-Least-Squares-Enhanced 3D Object Detection for 4D mmWave Radar, Remote Sensing 17(8):1465, 2025 — MDPI
16. RadarOcc（NeurIPS 2024）/ Radar-diffusion（ICRA 2024）— 雷达点云致密化的两条不同路线