yes fuking shit
This commit is contained in:
Horldsence
@@ -0,0 +1,388 @@
|
||||
"""位置×朝向联合可行集（第四问定向源）。
|
||||
|
||||
现状问题（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}
|
||||
Reference in New Issue
Block a user