障碍物遮挡判断算法
一. 背景
在感知和真值生成任务中，“遮挡”通常有两类不同问题：
3维空间遮挡
回答的问题是：从某个观察点出发，在三维空间里，一个目标是否被前方物体挡住。
图像遮挡
回答的问题是：在某个相机视角下，目标投影到图像后，是否在像素层面被前景物体遮挡。
二. 具体方法
1. 3维空间遮挡：射线法
原理
从观察点向目标发出一条射线，如果在到达目标前，先与其他物体相交，则目标被遮挡。
射线表达式：
R(t) = O + t * D, t >= 0
其中：
- O 是观察点
- D 是指向目标的单位方向向量
- t 是沿射线方向的距离
如果障碍物用 3D 盒子表示，就判断射线是否先穿过障碍物盒子，再到达目标。
优点 - 几何意义清晰
- 精度高
- 适合分析真实的空间可见性
缺点 - 目标和障碍物很多时，计算量较大
- 不适合直接做大规模像素级图像遮挡判断
适用场景 - LiDAR 原点下的障碍物遮挡判断
- 3D bbox 在空间中的可见性分析
- 体素网格中的空间遮挡检查
Demo 代码
importnumpyasnpdefnormalize(v):n=np.linalg.norm(v)ifn<1e-9:raiseValueError("zero-length vector")returnv/ndefray_aabb_intersect(origin,direction,box_center,box_size):""" 判断一条射线是否与 3D 盒子(AABB)相交。 如果相交，返回射线第一次进入盒子的距离 t_hit； 否则返回 None。 原理： 1. 盒子在 x/y/z 三个方向都有一个范围 2. 分别计算射线进入和离开每个轴范围的时刻 3. 如果三个轴的有效区间有交集，则射线穿过盒子 """half=box_size/2.0box_min=box_center-half box_max=box_center+half t_enter_all=[]t_exit_all=[]foriinrange(3):# direction[i] == 0 表示射线平行于该轴ifabs(direction[i])<1e-9:# 如果起点不在该轴范围内，则不可能相交iforigin[i]<box_min[i]ororigin[i]>box_max[i]:returnNonet_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：交点全部在射线起点后方ift_near>t_farort_far<0:returnNonereturnmax(t_near,0.0)defis_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=Nonenearest_name=Noneforname,center,sizeinobstacles:t_hit=ray_aabb_intersect(origin,direction,center,size)ift_hitisnotNoneandt_hit<t_target:ifnearest_hitisNoneort_hit<nearest_hit:nearest_hit=t_hit nearest_name=namereturn{"occluded":nearest_hitisnotNone,"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 法
原理
在图像中，遮挡本质上是“同一个像素位置上，谁离相机更近”。
因此做法是：
- 将目标投影到图像
- 计算目标在该像素上的深度
- 与当前帧图像对应像素的前景深度比较
- 如果目标更远，则被遮挡
这就是深度图 / Z-buffer 方法。
优点
- 与图像可见性定义一致
- 批量处理效率高
- 很适合判断 voxel、3D bbox 在图像中的遮挡情况
缺点 - 依赖可靠的深度图或前景深度
- 深度图稀疏或有空洞时，需要补洞或标记 unknown
适用场景 - 判断 3D bbox 在相机图像中是否被遮挡
- 判断全局 voxel 在当前帧相机中是否可见
- 为 OCC 真值增加 visible / occluded / unknown 属性
Demo 代码
importnumpyasnpdefproject_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_camifZ<=0:returnNoneu=fx*X/Z+cx v=fy*Y/Z+cyreturnu,v,Zdefis_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)ifprojisNone:return{"status":"behind_camera"}u,v,z=proj h,w=depth_map.shape u_int=int(round(u))v_int=int(round(v))ifu_int<0oru_int>=worv_int<0orv_int>=h:return{"status":"out_of_image"}d=depth_map[v_int,u_int]ifnotnp.isfinite(d):return{"status":"unknown"}ifz<=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.0cx,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)