障碍物遮挡判断算法

一. 背景

在感知和真值生成任务中，"遮挡"通常有两类不同问题：

3维空间遮挡

回答的问题是：从某个观察点出发，在三维空间里，一个目标是否被前方物体挡住。

图像遮挡

回答的问题是：在某个相机视角下，目标投影到图像后，是否在像素层面被前景物体遮挡。

二. 具体方法

1. 3维空间遮挡：射线法

原理

从观察点向目标发出一条射线，如果在到达目标前，先与其他物体相交，则目标被遮挡。

射线表达式：

R(t) = O + t * D, t >= 0

其中：

• O 是观察点
• D 是指向目标的单位方向向量
• t 是沿射线方向的距离
  如果障碍物用 3D 盒子表示，就判断射线是否先穿过障碍物盒子，再到达目标。
  优点
• 几何意义清晰
• 精度高
• 适合分析真实的空间可见性
  缺点
• 目标和障碍物很多时，计算量较大
• 不适合直接做大规模像素级图像遮挡判断
  适用场景
• LiDAR 原点下的障碍物遮挡判断
• 3D bbox 在空间中的可见性分析
• 体素网格中的空间遮挡检查
  Demo 代码

import numpy as np
def normalize(v):
    n = np.linalg.norm(v)
    if n < 1e-9:
        raise ValueError("zero-length vector")
    return v / n
def ray_aabb_intersect(origin, direction, box_center, box_size):
    """
    判断一条射线是否与 3D 盒子(AABB)相交。
    如果相交，返回射线第一次进入盒子的距离 t_hit；
    否则返回 None。
    原理：
    1. 盒子在 x/y/z 三个方向都有一个范围
    2. 分别计算射线进入和离开每个轴范围的时刻
    3. 如果三个轴的有效区间有交集，则射线穿过盒子
    """
    half = box_size / 2.0
    box_min = box_center - half
    box_max = box_center + half
    t_enter_all = []
    t_exit_all = []
    for i in range(3):
        # direction[i] == 0 表示射线平行于该轴
        if abs(direction[i]) < 1e-9:
            # 如果起点不在该轴范围内，则不可能相交
            if origin[i] < box_min[i] or origin[i] > box_max[i]:
                return None
            t_enter_all.append(-np.inf)
            t_exit_all.append(np.inf)
        else:
            t1 = (box_min[i] - origin[i]) / direction[i]
            t2 = (box_max[i] - origin[i]) / direction[i]
            t_enter_all.append(min(t1, t2))
            t_exit_all.append(max(t1, t2))
    t_near = max(t_enter_all)
    t_far = min(t_exit_all)
    # t_near > t_far：三个轴范围没有公共交集
    # t_far < 0：交点全部在射线起点后方
    if t_near > t_far or t_far < 0:
        return None
    return max(t_near, 0.0)
def is_target_occluded(origin, target_point, obstacles):
    """
    判断目标点是否被前方障碍物遮挡。
    原理：
    1. 从 origin 朝 target_point 构造射线
    2. 目标点距离为 t_target
    3. 若某个障碍物的命中距离 t_hit < t_target，说明先撞到障碍物
    """
    vec = target_point - origin
    t_target = np.linalg.norm(vec)
    direction = normalize(vec)
    nearest_hit = None
    nearest_name = None
    for name, center, size in obstacles:
        t_hit = ray_aabb_intersect(origin, direction, center, size)
        if t_hit is not None and t_hit < t_target:
            if nearest_hit is None or t_hit < nearest_hit:
                nearest_hit = t_hit
                nearest_name = name
    return {
        "occluded": nearest_hit is not None,
        "target_distance": t_target,
        "nearest_hit": nearest_hit,
        "occluder": nearest_name,
    }
示例：
观察点在原点，目标在 (6,0,0)
box_A 在 (3,0,0)，正好挡在前面
origin = np.array([0.0, 0.0, 0.0])
target = np.array([6.0, 0.0, 0.0])
obstacles = [
    ("box_A", np.array([3.0, 0.0, 0.0]), np.array([1.0, 1.0, 1.0])),
    ("box_B", np.array([3.0, 2.0, 0.0]), np.array([1.0, 1.0, 1.0])),
]
result = is_target_occluded(origin, target, obstacles)
print(result)

2. 图像遮挡：深度图 / Z-buffer 法

原理

在图像中，遮挡本质上是"同一个像素位置上，谁离相机更近"。

因此做法是：

1. 将目标投影到图像
2. 计算目标在该像素上的深度
3. 与当前帧图像对应像素的前景深度比较
4. 如果目标更远，则被遮挡
   这就是深度图 / Z-buffer 方法。
   优点

• 与图像可见性定义一致
• 批量处理效率高
• 很适合判断 voxel、3D bbox 在图像中的遮挡情况
  缺点
• 依赖可靠的深度图或前景深度
• 深度图稀疏或有空洞时，需要补洞或标记 unknown
  适用场景
• 判断 3D bbox 在相机图像中是否被遮挡
• 判断全局 voxel 在当前帧相机中是否可见
• 为 OCC 真值增加 visible / occluded / unknown 属性
  Demo 代码

import numpy as np
def project_point_to_image(point_cam, fx, fy, cx, cy):
    """
    将相机坐标系中的 3D 点投影到图像。
    原理：
    相机模型中，3D 点 (X, Y, Z) 投影到像素 (u, v)：
        u = fx * X / Z + cx
        v = fy * Y / Z + cy
    其中 Z 就是该点到相机的深度。
    """
    X, Y, Z = point_cam
    if Z <= 0:
        return None
    u = fx * X / Z + cx
    v = fy * Y / Z + cy
    return u, v, Z
def is_point_occluded_in_image(point_cam, depth_map, fx, fy, cx, cy, eps=0.1):
    """
    判断一个 3D 点在当前图像中是否被遮挡。
    原理：
    1. 先把 3D 点投影到图像像素 (u, v)
    2. 取出这个像素的前景深度 d
    3. 如果点自己的深度 z > d + eps，说明前面已有更近物体，它被遮挡
    4. 否则认为该点可见
    """
    proj = project_point_to_image(point_cam, fx, fy, cx, cy)
    if proj is None:
        return {"status": "behind_camera"}
    u, v, z = proj
    h, w = depth_map.shape
    u_int = int(round(u))
    v_int = int(round(v))
    if u_int < 0 or u_int >= w or v_int < 0 or v_int >= h:
        return {"status": "out_of_image"}
    d = depth_map[v_int, u_int]
    if not np.isfinite(d):
        return {"status": "unknown"}
    if z <= d + eps:
        return {"status": "visible", "z_target": z, "z_front": d}
    else:
        return {"status": "occluded", "z_target": z, "z_front": d}
示例：
假设当前图像大小 640x480
某个像素位置的前景深度为 4.2m
depth_map = np.full((480, 640), np.inf, dtype=np.float32)
depth_map[260, 420] = 4.2
相机内参
fx, fy = 500.0, 500.0
cx, cy = 320.0, 240.0
一个 3D 点在相机坐标系中的位置
它会投影到大约 (420, 260)，自身深度是 5.0m
point_cam = np.array([1.0, 0.2, 5.0])
result = is_point_occluded_in_image(point_cam, depth_map, fx, fy, cx, cy)
print(result)