389 lines
19 KiB
Python
389 lines
19 KiB
Python
"""位置×朝向联合可行集（第四问定向源）。
|
||
|
||
现状问题（docs/第三四问架构优化与运行提速方案.md §1.3 F3）：第四问的失败**全部**
|
||
来自定向源（评测机两局：全向 17/17 清除，定向仅 2/6）。原因不是覆盖不足，而是
|
||
策略只知道"源可能在某个空间区域"，不知道"它朝着哪边"——于是选点只能靠固定
|
||
45° 阶梯试探，一旦试探失败就退化到 ≤400 格 20 m 盲清。
|
||
|
||
本模块把"看不看得见"变成可推理的量：为已在 Ω 上维护的网格幸存集
|
||
（:class:`~mcm26b.particles.ChannelParticles`）附加每格 24 档朝向可行性掩码，
|
||
得到 (位置 × 朝向) 联合可行集。观测语义严格对齐评测机
|
||
``referee.simulator._observe``：
|
||
|
||
- 距离 > r_eff → no_signal；定向且源→读点方向与朝向夹角 > 90° → no_signal；
|
||
- 距离 ≤ 5 m → near（**仍需可见**：3 m 处背向的定向源同样返回 no_signal）；
|
||
- 其余 → direction（读数 = 源→读点方位 + 有界误差）。
|
||
|
||
更新规则（全部保守，只删"必然不可能"的格/档）：
|
||
|
||
=============== ============================================ ==================
|
||
观测 位置 朝向
|
||
=============== ============================================ ==================
|
||
``direction``@s 距离 ≤ 1500、角向 |gap| ≤ δ_eff + 格裕量 必须**可能**覆盖 s
|
||
``near``@s 距离 ≤ 5 必须**可能**覆盖 s
|
||
``no_signal``@s **不删**（可能是朝向遮挡或距离不足） 必须**确定不**覆盖 s
|
||
（仅当整格必在 r_eff_min 内才约束朝向）
|
||
=============== ============================================ ==================
|
||
|
||
阈值全部取保守侧，并计入**格内角裕量**
|
||
``asin(margin / max(dist − margin, 1))``（格内任一点相对格心方位偏差上界）：
|
||
|
||
- "必须可能覆盖"：``gap ≤ 90° + 半档 + 查表舍入 + 格内角裕量``（宁可保留）；
|
||
- "必须确定不覆盖"：先删去 ``gap ≤ 90° − 半档 − 查表舍入 − 格内角裕量`` 的档，
|
||
只保留确定看不见 s 的档（同样宁可保留）。
|
||
|
||
因此真源 (位置, 朝向) 永不被删——这是网格幸存集真源保留定理的二维推广。
|
||
（2026-09-12 修正：no_signal 分支原先误用"确定覆盖"掩码做交集，等价于把
|
||
规则反向；keep=True 分支也漏计了格内角裕量。两处都会删掉真源朝向档，
|
||
现由 ``tests/test_joint_belief.py`` 的随机观测序列保留性测试固化。）
|
||
|
||
用途：
|
||
1. ``prob_detect``：该点能看见该频道任一可行 (位置, 朝向) 的概率质量，
|
||
用于**可见性选点**（取代固定阶梯）——只影响选点顺序，不承担安全责任；
|
||
2. ``expected_posterior_radius``：一次测量后的期望后验 MEC 半径（D1 值函数）；
|
||
3. ``is_absent``：联合集空 ⟹ 该频道不存在源（严格，与覆盖证明同一条件）；
|
||
4. 位置边际仍由凸证书负责 20 m 清除判定，本模块不改变任何证书口径。
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import math
|
||
|
||
from ..config import ProblemConfig, Vec2
|
||
from ..particles import ChannelParticles, _branch_radius_array
|
||
from ..protocol.rules import DIRECTION, NEAR, NO_SIGNAL
|
||
|
||
HEADING_BINS = 24 # 15° 一档
|
||
_BIN_HALF_DEG = 180.0 / HEADING_BINS # 7.5°
|
||
_FULL_MASK = (1 << HEADING_BINS) - 1
|
||
_GRID_DEG = 3.0 # 角度查表分辨率
|
||
_GRID_N = int(round(360.0 / _GRID_DEG))
|
||
|
||
try: # numpy 仅作可选加速后端（与 particles 一致）
|
||
import numpy as _np
|
||
from ..particles import _np as _particles_np
|
||
if _particles_np is None: # MCM26B_NO_NUMPY 或缺失：强制纯 Python
|
||
_np = None
|
||
except Exception: # pragma: no cover - 环境无 numpy
|
||
_np = None
|
||
|
||
|
||
def heading_gap(a_deg: float, b_deg: float) -> float:
|
||
"""两个朝向角的最小夹角（度）。"""
|
||
return abs((a_deg - b_deg + 180.0) % 360.0 - 180.0)
|
||
|
||
|
||
def _cover_mask(bearing_deg: float, half_deg: float) -> int:
|
||
"""与该方位夹角不超过 ``half_deg`` 的朝向档掩码。"""
|
||
mask = 0
|
||
for k in range(HEADING_BINS):
|
||
if heading_gap(360.0 * k / HEADING_BINS, bearing_deg) <= half_deg:
|
||
mask |= 1 << k
|
||
return mask
|
||
|
||
|
||
# 查表：以 bearing 为轴、半角 90° 的可见半平面。
|
||
# - COVER：**可能**覆盖该方位（含半档与查表舍入的保守侧）；
|
||
# - SILENT：**确定**覆盖该方位（内缩半档与舍入）——离散对照表，供审计与
|
||
# 文档比对；实际裁剪走 _cover_mask / _far_mask，因为还必须逐格计入
|
||
# "格内角裕量"（格心到格内任一点的方位偏差上界），无法预先制表。
|
||
_COVER_TABLE = tuple(
|
||
_cover_mask(_GRID_DEG * i, 90.0 + _BIN_HALF_DEG + _GRID_DEG / 2)
|
||
for i in range(_GRID_N))
|
||
_SILENT_TABLE = tuple(
|
||
_cover_mask(_GRID_DEG * i, 90.0 - _BIN_HALF_DEG - _GRID_DEG / 2)
|
||
for i in range(_GRID_N))
|
||
|
||
|
||
def _far_mask(bearing_deg: float, exclude_half_deg: float) -> int:
|
||
"""与方位夹角**大于** ``exclude_half_deg`` 的朝向档掩码（no_signal 用）。
|
||
|
||
与 :func:`_cover_mask` 互补：no_signal 只允许保留"确定看不见"的朝向档。
|
||
"""
|
||
mask = 0
|
||
for k in range(HEADING_BINS):
|
||
if heading_gap(360.0 * k / HEADING_BINS, bearing_deg) > exclude_half_deg:
|
||
mask |= 1 << k
|
||
return mask
|
||
|
||
if _np is not None: # 12 位半字 popcount 查表（朝向掩码 24 位）与半平面查表数组
|
||
_POP16 = _np.array([bin(value).count("1") for value in range(1 << 12)],
|
||
dtype=_np.int64)
|
||
_COVER_ARR = _np.array(_COVER_TABLE, dtype=_np.int64)
|
||
else: # pragma: no cover - 无 numpy 分支
|
||
_POP16 = _COVER_ARR = None
|
||
|
||
|
||
def _popcount(mask: int) -> int:
|
||
return bin(mask).count("1")
|
||
|
||
|
||
def _table_index(bearing_deg: float) -> int:
|
||
return int(round((bearing_deg % 360.0) / _GRID_DEG)) % _GRID_N
|
||
|
||
|
||
class JointBelief:
|
||
"""单频道 (位置 × 朝向) 联合可行集。"""
|
||
|
||
def __init__(self, config: ProblemConfig = ProblemConfig()):
|
||
self.config = config
|
||
self.field = config.field
|
||
self.position = ChannelParticles(config)
|
||
self.masks: dict[int, int] = {} # 格索引 -> 朝向掩码（缺省为全档）
|
||
self._sample_cache: tuple[int, list[int]] | None = None
|
||
|
||
# ------------------------------------------------------------ 查询
|
||
def __len__(self) -> int:
|
||
return len(self.position)
|
||
|
||
def is_absent(self) -> bool:
|
||
"""联合集空 ⟹ 该频道不存在源（真源保留定理的二维推广）。"""
|
||
return not self.position.alive
|
||
|
||
def _mask(self, index: int) -> int:
|
||
return self.masks.get(index, _FULL_MASK)
|
||
|
||
def sample(self, size: int = 256) -> list[int]:
|
||
"""均匀抽样幸存格（评分用；证书始终用完整集合）。"""
|
||
cached = self._sample_cache
|
||
if cached is not None and cached[0] == self.position.version:
|
||
return cached[1]
|
||
alive = sorted(self.position.alive)
|
||
if len(alive) > size:
|
||
step = len(alive) / size
|
||
alive = [alive[int(k * step) % len(alive)] for k in range(size)]
|
||
self._sample_cache = (self.position.version, alive)
|
||
return alive
|
||
|
||
def _mask_popcount(self, index: int) -> int:
|
||
return _popcount(self._mask(index))
|
||
|
||
def expected_posterior_radius(self, reader: Vec2, size: int | None = None) -> float:
|
||
"""一次测量后的期望后验 MEC 半径（米）；D1 值函数。
|
||
|
||
与 :meth:`ChannelParticles.expected_posterior_radius` 同族，差别只在
|
||
no_signal 分支概率：定向源不仅会因距离 > r_eff 静默，还会因朝向遮挡
|
||
静默，因此
|
||
|
||
p_vis(p) = P(r_eff ≥ |p−q|) × cover_ratio(p, q)
|
||
|
||
其中 ``cover_ratio`` 是该格朝向掩码中"可能覆盖 q 方位"的档位占比。
|
||
分支半径语义：
|
||
|
||
- direction（可见，非 near）：读数为 q 处的方位 ±δ，后验收缩到该方位
|
||
楔形与当前可行集之交，半径取幸存子采样的质心最大距离；
|
||
- no_signal：定向源的静默**不删空间候选**，位置后验半径保持当前值；
|
||
- near：后验为 5m 盘。
|
||
|
||
返回值只用于**选点排序**，不参与任何安全判定；证书仍由凸/网格口径给出。
|
||
"""
|
||
pf = self.position
|
||
if not pf.alive:
|
||
return 0.0
|
||
if size is None:
|
||
size = self.config.bayes.score_subsample
|
||
indices = self.sample(max(2, size))
|
||
if not indices:
|
||
return 0.0
|
||
stride = max(len(indices) // max(self.config.bayes.mc_representatives, 1), 1)
|
||
representatives = indices[::stride]
|
||
if _np is not None:
|
||
return self._epr_vector(reader, indices, representatives)
|
||
return self._epr_python(reader, indices, representatives) # pragma: no cover
|
||
|
||
def _epr_vector(self, reader: Vec2, indices: list[int],
|
||
representatives: list[int]) -> float:
|
||
pf = self.position
|
||
points = _np.array([pf.cell_center(index) for index in indices], dtype=float)
|
||
masks = _np.array([self._mask(index) for index in indices], dtype=_np.int64)
|
||
reps = _np.array([pf.cell_center(index) for index in representatives],
|
||
dtype=float)
|
||
rep_masks = _np.array([self._mask(index) for index in representatives],
|
||
dtype=_np.int64)
|
||
dx, dy = points[:, 0] - reader[0], points[:, 1] - reader[1]
|
||
dist = _np.hypot(dx, dy)
|
||
bearing = _np.degrees(_np.arctan2(dy, dx))
|
||
rdx, rdy = reps[:, 0] - reader[0], reps[:, 1] - reader[1]
|
||
rdist = _np.hypot(rdx, rdy)
|
||
rbearing = _np.degrees(_np.arctan2(rdy, rdx))
|
||
cover = _COVER_ARR[_np.round(_np.mod(rbearing, 360.0) / _GRID_DEG)
|
||
.astype(_np.int64) % _GRID_N]
|
||
visible_mask = rep_masks & cover
|
||
bits = (_POP16[visible_mask & 0xFFF]
|
||
+ _POP16[(visible_mask >> 12) & 0xFFF])
|
||
cover_ratio = bits / float(HEADING_BINS)
|
||
p_silent_range = _np.clip((rdist - self.field.r_eff_min_m)
|
||
/ (self.field.r_eff_max_m
|
||
- self.field.r_eff_min_m), 0.0, 1.0)
|
||
p_vis = (1.0 - p_silent_range) * cover_ratio
|
||
p_ns = 1.0 - p_vis
|
||
gaps = _np.abs((bearing[None, :] - rbearing[:, None] + 180.0) % 360.0 - 180.0)
|
||
in_wedge = ((gaps <= self.config.field.bearing_error_half_deg
|
||
+ self.config.search.bearing_rounding_margin_deg + 1e-9)
|
||
& (dist[None, :] <= self.field.r_eff_max_m + pf.margin_m))
|
||
eps_radius = pf.spacing_m / 2.0
|
||
radii = []
|
||
for row in in_wedge:
|
||
member = points[row]
|
||
radii.append(_branch_radius_array(member)
|
||
if len(member) else eps_radius)
|
||
radii = _np.array(radii, dtype=float)
|
||
current = pf.mec()[1]
|
||
near = rdist <= self.field.near_radius_m + pf.margin_m
|
||
values = _np.where(near, self.field.near_radius_m + pf.margin_m,
|
||
p_ns * current + p_vis * radii)
|
||
return float(values.mean())
|
||
|
||
def _epr_python(self, reader: Vec2, indices: list[int],
|
||
representatives: list[int]) -> float: # pragma: no cover
|
||
"""纯 Python 回退（与向量版逐分支同语义，供无 numpy 环境与对拍）。"""
|
||
pf = self.position
|
||
points = [pf.cell_center(index) for index in indices]
|
||
masks = [self._mask(index) for index in indices]
|
||
eps_radius = pf.spacing_m / 2.0
|
||
current = pf.mec()[1]
|
||
half_angle = (self.config.field.bearing_error_half_deg
|
||
+ self.config.search.bearing_rounding_margin_deg)
|
||
total = 0.0
|
||
for rep_index in representatives:
|
||
rx, ry = pf.cell_center(rep_index)
|
||
distance = math.hypot(rx - reader[0], ry - reader[1])
|
||
if distance <= self.field.near_radius_m + pf.margin_m:
|
||
total += self.field.near_radius_m + pf.margin_m
|
||
continue
|
||
reading = math.degrees(math.atan2(ry - reader[1], rx - reader[0]))
|
||
cover = _COVER_TABLE[_table_index(reading)]
|
||
bits = _popcount(self._mask(rep_index) & cover)
|
||
ratio = bits / float(HEADING_BINS)
|
||
if distance <= self.field.r_eff_min_m:
|
||
p_range = 1.0
|
||
elif distance >= self.field.r_eff_max_m:
|
||
p_range = 0.0
|
||
else:
|
||
p_range = ((self.field.r_eff_max_m - distance)
|
||
/ (self.field.r_eff_max_m - self.field.r_eff_min_m))
|
||
p_vis = p_range * ratio
|
||
members = []
|
||
for point, mask in zip(points, masks):
|
||
if math.dist(point, reader) > self.field.r_eff_max_m + pf.margin_m:
|
||
continue
|
||
bearing = math.degrees(math.atan2(point[1] - reader[1],
|
||
point[0] - reader[0]))
|
||
if heading_gap(bearing, reading) <= half_angle + 1e-9:
|
||
members.append(point)
|
||
if members:
|
||
mx = sum(p[0] for p in members) / len(members)
|
||
my = sum(p[1] for p in members) / len(members)
|
||
radius = max(math.hypot(p[0] - mx, p[1] - my) for p in members)
|
||
else:
|
||
radius = eps_radius
|
||
total += (1.0 - p_vis) * current + p_vis * radius
|
||
return total / max(len(representatives), 1)
|
||
|
||
|
||
def prob_detect(self, reader: Vec2, size: int = 256) -> float:
|
||
"""在 ``reader`` 处能看见任一可行 (位置, 朝向) 的概率质量（启发式）。
|
||
|
||
r_eff ~ U[1000, 1500] 解析积分；朝向按可行档位占比计权。
|
||
只用于选点排序，不参与任何安全判定。
|
||
"""
|
||
cells = self.sample(size)
|
||
if not cells:
|
||
return 0.0
|
||
total = 0.0
|
||
span = self.field.r_eff_max_m - self.field.r_eff_min_m
|
||
for index in cells:
|
||
gx, gy = self.position.cell_center(index)
|
||
distance = math.hypot(reader[0] - gx, reader[1] - gy)
|
||
if distance > self.field.r_eff_max_m:
|
||
continue
|
||
if distance <= self.field.r_eff_min_m:
|
||
p_range = 1.0
|
||
else:
|
||
p_range = (self.field.r_eff_max_m - distance) / span
|
||
if p_range <= 0.0:
|
||
continue
|
||
bearing = math.degrees(math.atan2(reader[1] - gy, reader[0] - gx))
|
||
cover = _COVER_TABLE[_table_index(bearing)]
|
||
bits = bin(self._mask(index) & cover).count("1")
|
||
if bits:
|
||
total += p_range * bits / HEADING_BINS
|
||
return total / len(cells)
|
||
|
||
# ------------------------------------------------------------ 观测更新
|
||
def _apply_heading(self, station: Vec2, keep: bool,
|
||
within_m: float | None = None) -> None:
|
||
"""按可见性约束裁剪朝向档（保守：只删"必然不可能"的档）。
|
||
|
||
``keep=True``（direction / near）⟹ 朝向必须**可能**覆盖 station：
|
||
保留与方位夹角 ≤ 90° + 半档 + 查表舍入 + 格内角裕量的档。
|
||
|
||
``keep=False``（no_signal 且整格必在 r_eff_min 内）⟹ 朝向必须
|
||
**确定不覆盖** station：只保留与方位夹角 > 90° − 半档 − 查表舍入
|
||
− 格内角裕量的档。
|
||
|
||
``格内角裕量 = asin(margin / max(dist − margin, 1))``：格内任一点
|
||
相对格心的方位偏差上界。两个方向都必须计入它，否则会误删真源的
|
||
朝向档（keep=True 漏计）或保留必然可见的档（keep=False 漏计）。
|
||
"""
|
||
margin_m = self.position.margin_m
|
||
doomed: list[int] = []
|
||
for index in self.position.alive:
|
||
gx, gy = self.position.cell_center(index)
|
||
dx, dy = station[0] - gx, station[1] - gy
|
||
distance = math.hypot(dx, dy)
|
||
if within_m is not None and distance > within_m:
|
||
continue
|
||
bearing = math.degrees(math.atan2(dy, dx))
|
||
if distance <= margin_m + 1.0:
|
||
# 格贴测点：格内方位可指向任意方向，任何一侧都不可据此裁剪
|
||
continue
|
||
angular_margin = math.degrees(
|
||
math.asin(min(0.9999, margin_m / (distance - margin_m))))
|
||
if keep:
|
||
updated = self._mask(index) & _cover_mask(
|
||
bearing, 90.0 + _BIN_HALF_DEG + _GRID_DEG / 2 + angular_margin)
|
||
else:
|
||
updated = self._mask(index) & _far_mask(
|
||
bearing, 90.0 - _BIN_HALF_DEG - _GRID_DEG / 2 - angular_margin)
|
||
if updated == 0:
|
||
doomed.append(index)
|
||
self.masks.pop(index, None)
|
||
elif updated != _FULL_MASK:
|
||
self.masks[index] = updated
|
||
if doomed:
|
||
self.position.discard(doomed)
|
||
|
||
def apply(self, station: Vec2, result: str,
|
||
svd_deg: float | None = None) -> None:
|
||
"""按读数更新联合可行集（保守，真源 (位置, 朝向) 永不被删）。"""
|
||
if result == DIRECTION:
|
||
self.position.apply_measure(station, DIRECTION, svd_deg,
|
||
directional=True)
|
||
self._apply_heading(station, keep=True)
|
||
elif result == NEAR:
|
||
# near 同样要求可见（评测机先判朝向再判距离）
|
||
self.position.apply_measure(station, NEAR, directional=True)
|
||
self._apply_heading(station, keep=True)
|
||
elif result == NO_SIGNAL:
|
||
# 距离阈值必须取**保守侧**：只有"整格都必然落在 r_eff_min 圆盘内"
|
||
# 时，no_signal 才一定由朝向遮挡解释。格心到格内任一点 ≤ margin，
|
||
# 故阈值取 r_eff_min − margin；取 + margin 会把 1000~1010m 的模糊带
|
||
# 也当成"必须不覆盖"，从而可能删掉真源的朝向档（破坏真源保留性）。
|
||
self._apply_heading(station, keep=False,
|
||
within_m=max(
|
||
0.0, self.field.r_eff_min_m
|
||
- self.position.margin_m))
|
||
else:
|
||
raise ValueError(f"未知测向结果: {result!r}")
|
||
|
||
# ------------------------------------------------------------ 诊断
|
||
def summary(self) -> dict:
|
||
"""联合集规模快照（写入策略 summary，供审计与论文复算）。"""
|
||
cells = self.sample(256)
|
||
bins = [bin(self._mask(index)).count("1") for index in cells]
|
||
return {"cells": len(self.position),
|
||
"sampled_mean_heading_bins": (round(sum(bins) / len(bins), 2)
|
||
if bins else 0.0),
|
||
"heading_bins_total": HEADING_BINS}
|