自瞄教程 / 实验材料
PnP 坐标误差实验材料
可从教程站直接查看的 PnP 受控采集清单与分析脚本。 此页面随教程站静态发布，不依赖 GitHub 仓库权限。
README.md
# PnP 三维坐标误差教学实验
这组实验围绕三个连续的教学问题展开：
1. 检测器原始角点相对同曝光真值呈现怎样的二维分布；
2. 角点误差怎样传播为 PnP 的 `x/y/z` 误差；
3. 这些误差是否满足 EKF 常用的独立、固定高斯噪声近似。
> 证据范围修正（2026-08）：这是一组历史教学采集，不是当前自瞄 B 的总体 PnP 分布。三组数据按唯一曝光时间戳反算，每秒约有 `60.8 / 57.7 / 56.9` 个成功匹配到 PnP 候选的曝光事件；无观测、空候选和未匹配事件不在下方 CSV 的空间误差分布中。图表适合解释几何传播机制，数值不能直接作为生产 EKF 的固定协方差。
## 受控条件
- 历史 Daedalus Simulator Release `1.0.1`（不是当前 Linux `1.3.1` 采集事实源）；
- 原生靶场 3 号目标，车体位置固定；
- `6 mm` 镜头，`1440 × 1080`；
- 自转速度固定为 `30°/s`；
- 场景设置距离为 `2 m`、`4 m`、`6 m`；
- 每组采集 `20 s`；
- PnP 观测与模拟器真值按生产者 epoch、帧序号和曝光时间严格对应。
| 距离 | 唯一匹配曝光 | 时间跨度 | 有效匹配事件率 | 曝光间隔 p50 / p95 / max |
| ---: | ---: | ---: | ---: | ---: |
| `2 m` | 1251 | 20.56 s | 60.8 Hz | 12.0 / 37.3 / 187.7 ms |
| `4 m` | 1183 | 20.48 s | 57.7 Hz | 15.5 / 29.3 / 90.5 ms |
| `6 m` | 1168 | 20.51 s | 56.9 Hz | 15.0 / 33.1 / 177.8 ms |
“有效匹配事件率”不是相机、渲染器或模拟器配置 FPS。一帧可有多个候选，漏检和无效事件也会让有效率下降；任何时序分析都应使用 `timestamp_ns`，而不是用 CSV 行号或固定帧率补时间。
## 与当前证据的关系
当前自瞄 B 以 Linux x86_64 Daedalus Simulator / SDK `1.3.1` 为正式采集入口。权威登记保留在自瞄仓库的 `trajectory_evidence_chain.md`、`pnp_evidence_registry.json`、`timeseries_evidence_registry.json` 及各实验 manifest 中。公开教程只摘录可复核的汇总，不复制受保护 raw RGBA、完整 truth、checkpoint 或不可再生数据。
当前证据已经确认：
- 120-run 中有 `177483` 个 valid event，典型间隔约 `7.94 ms`，但 p95 约 `23.5–24.0 ms`，并存在数百毫秒和整轮零观测；
- exact projected corners 进入同一 IPPE/坐标链时，3D p95 为 `0.009 mm`；
- `77518` 条 truth-visible 配对观测的视线径向 p95 为 `0.81–1.01 m`，水平 p95 为 `6.1–7.5 mm`；
- 两 session 的 same-frame 角点修复 smoke 没有通过独立 session 泛化，当前不得替换生产 PnP。
### 最新纯旋转轨迹几何结论
`analyze_latest_trajectory_geometry.py` 对最新 56-session 配对表重新做了轨迹形状检验。它只取纯旋转数据，共 `63029` 条同曝光配对、`18` 个独立 session 和 `72` 个 session-slot 组，并校验输入 SHA-256、重复键、缺失值和非有限数值。
新结果不支持“真实圆弧会被 PnP 稳定地观测成椭圆”。相机射线夹角 p95 为 `0.104°`，但视线深度误差 p95 为 `0.814 m`；中心相对半径误差与视线深度误差绝对值的相关系数为 `0.942`。椭圆模型在跨重复 session 的 RMSE 上只比圆模型低约 `3%`，尾部没有改善，并在连续相位留出中劣于圆模型。因此当前形变应解释为距离和相位相关的深度径向误差，而不是稳定椭圆。
完整方法、质量检查和分组统计见 `latest-trajectory-geometry-eda.md`，机器可读汇总见 `results/latest-trajectory-geometry-summary.json`。教程图 `public/images/pose-estimation/pnp-latest-trajectory-geometry.{png,svg}` 直接绘制固定工况下的逐帧俯视散点，并叠加真值圆弧、圆拟合和椭圆拟合。
因此，历史图保留“深度弱约束、长尾、相关噪声”的教学价值，但必须和当前大规模、真实时间、显式缺失的证据分开解释。
`manifests/` 保存三组采集清单。原始 observation/truth JSONL 体积较大，保留在采集工作区；`results/summary.json` 和 `results/noise-summary.json` 记录了每组原始流的 SHA-256，可用于确认数据来源。
主要处理结果包括：
- `matched-samples.csv`：三维坐标与观测轨迹模型对照的逐观测配对结果；
- `corner-noise-samples.csv`：逐角点真值、原始角点、修正角点及二维残差；
- `observation-noise-samples.csv`：逐 PnP 观测的 `x/y/z`、视线方向和投影尺度误差；
- `noise-summary.json`：分位数、协方差、偏度、峰度、尺度—深度相关性和自相关；
- `eda-report.md`：面向后续 EKF 建模的探索性分析报告。
## 在 Linux 重新绘图
当前工作环境优先使用 Linux。先准备依赖：
```bash
python3 -m venv .venv
source .venv/bin/activate
python -m pip install --upgrade pip
python -m pip install numpy matplotlib scipy
```
然后重新生成观测轨迹模型对照和三维坐标教学图：
```bash
python experiments/pnp-coordinate-error/build_figures.py \
--session "2=<2m session_result.json>" \
--session "4=<4m session_result.json>" \
--session "6=<6m session_result.json>" \
--output-dir experiments/pnp-coordinate-error/results \
--image-dir public/images/pose-estimation \
--arc-distance 4
```
重新生成角点、坐标分布与 EKF 铺垫图：
```bash
python experiments/pnp-coordinate-error/build_noise_distributions.py \
--session "2=<2m session_result.json>" \
--session "4=<4m session_result.json>" \
--session "6=<6m session_result.json>" \
--output-dir experiments/pnp-coordinate-error/results \
--image-dir public/images/pose-estimation
```
用最新纯旋转证据重新生成轨迹几何结论、汇总和教程图：
```bash
python experiments/pnp-coordinate-error/analyze_latest_trajectory_geometry.py \
--input /path/to/paired_trajectory_rows.csv \
--expected-sha256 c8368b7441fd54da98cbb7f2be79ccacb38887b1b9a3e55e9d0eb638c4c45ef7 \
--summary-output experiments/pnp-coordinate-error/results/latest-trajectory-geometry-summary.json \
--report-output experiments/pnp-coordinate-error/latest-trajectory-geometry-eda.md \
--figure-output-base public/images/pose-estimation/pnp-latest-trajectory-geometry
```
## 在 Windows PowerShell 重新绘图
```powershell
python .\experiments\pnp-coordinate-error\build_figures.py `
--session "2=<2m session_result.json>" `
--session "4=<4m session_result.json>" `
--session "6=<6m session_result.json>" `
--output-dir .\experiments\pnp-coordinate-error\results `
--image-dir .\public\images\pose-estimation `
--arc-distance 4
```
脚本使用装甲板朝向完成物理板配对，再在跟踪坐标系中计算误差。`x` 为前向、`y` 为横向、`z` 为竖直；目标位于相机中心附近时，`x` 近似对应相机深度。轨迹图中的一阶相位谐波使用离线 truth 相位，只是本段可见数据的描述性候选模型，不是可在线使用的拟合器；它不用于宣称真实圆弧必然被观测成椭圆，也不替代圆、椭圆和一般轨迹模型的跨 session 比较。
### 重新生成角点、坐标分布与 EKF 铺垫图
```powershell
python .\experiments\pnp-coordinate-error\build_noise_distributions.py `
--session "2=<2m session_result.json>" `
--session "4=<4m session_result.json>" `
--session "6=<6m session_result.json>" `
--output-dir .\experiments\pnp-coordinate-error\results `
--image-dir .\public\images\pose-estimation
```
这一步按照图像中心完成物理装甲板的一对一关联，再投影精确物理角点。角点顺序、角点误差和 PnP 坐标误差均来自同一曝光，不通过误差大小挑选样本。
build_figures.py
#!/usr/bin/env python3
"""Build the teaching figures for PnP coordinate error propagation.
The input is the paired Stage-3 observation/truth stream produced by the
official simulator release. Simulator truth is used only for evaluation:
each PnP observation is matched one-to-one to the closest physical armor in
the same exposure, then all errors are computed in the tracker/chassis frame.
"""
from __future__ import print_function
import argparse
import csv
import hashlib
import itertools
import json
import math
import os
from collections import defaultdict
import matplotlib
matplotlib.use("Agg")
import matplotlib.font_manager as font_manager # noqa: E402
import matplotlib.pyplot as plt # noqa: E402
import numpy as np # noqa: E402
AXIS_COLORS = {
"x": "#0F4D92",
"y": "#D97706",
"z": "#2E8B57",
}
def load_json(path):
with open(path, "r", encoding="utf-8-sig") as handle:
return json.load(handle)
def iter_jsonl(path):
with open(path, "r", encoding="utf-8") as handle:
for line in handle:
if line.strip():
yield json.loads(line)
def quat_rotate(quaternion_wxyz, vector):
quaternion = np.asarray(quaternion_wxyz, dtype=np.float64)
vector = np.asarray(vector, dtype=np.float64)
quaternion = quaternion / np.linalg.norm(quaternion)
w = quaternion[0]
qv = quaternion[1:]
return (
(w * w - np.dot(qv, qv)) * vector
+ 2.0 * qv * np.dot(qv, vector)
+ 2.0 * w * np.cross(qv, vector)
)
def quat_inverse_rotate(quaternion_wxyz, vector):
inverse = np.asarray(quaternion_wxyz, dtype=np.float64).copy()
inverse[1:] *= -1.0
return quat_rotate(inverse, vector)
def truth_target(truth_row, target_number, tracker_origin):
selected_target_id = int(truth_row["selected_target_id"])
targets = truth_row["ground_truth"]["targets"]
candidates = []
for target in targets:
if int(target.get("armor_label", -1)) != target_number:
continue
if int(target.get("target_id", -1)) != selected_target_id:
continue
center = np.asarray(target["world_position_m"], dtype=np.float64)
if np.linalg.norm(center - tracker_origin) < 1.0:
continue
candidates.append(target)
if len(candidates) != 1:
raise ValueError("expected exactly one selected target, got %d" % len(candidates))
return candidates[0]
def tracker_truth_geometry(truth_row, target_number):
exposure = truth_row["exposure_state"]
origin = np.asarray(exposure["gimbal_position_world_m"], dtype=np.float64)
observer_q = exposure["chassis_quaternion_world_wxyz"]
target = truth_target(truth_row, target_number, origin)
target_center_world = np.asarray(target["world_position_m"], dtype=np.float64)
target_q = target["world_quaternion_wxyz"]
target_center_tracker = quat_inverse_rotate(observer_q, target_center_world - origin)
plates = []
for armor in target.get("armors", []):
local = np.asarray(armor["chassis_local_position_m"], dtype=np.float64)
world = target_center_world + quat_rotate(target_q, local)
tracker = quat_inverse_rotate(observer_q, world - origin)
local_normal = np.asarray(armor["chassis_local_outward_normal"], dtype=np.float64)
world_normal = quat_rotate(target_q, local_normal)
tracker_normal = quat_inverse_rotate(observer_q, world_normal)
plates.append({
"slot": int(armor["relative_slot"]),
"tracker": tracker,
# Production armor yaw points from the plate toward the chassis,
# opposite to the simulator's outward-normal convention.
"yaw": math.atan2(float(-tracker_normal[1]), float(-tracker_normal[0])),
})
return target_center_tracker, plates
def assign_one_to_one(observations, truth_plates):
if not observations:
return []
if len(observations) > len(truth_plates):
observations = observations[:len(truth_plates)]
best = None
for plate_indices in itertools.permutations(range(len(truth_plates)), len(observations)):
cost = 0.0
pairs = []
for observation, plate_index in zip(observations, plate_indices):
plate = truth_plates[plate_index]
delta = observation["pnp"] - plate["tracker"]
position_cost = float(np.linalg.norm(delta))
angular_delta = math.atan2(
math.sin(observation["yaw"] - plate["yaw"]),
math.cos(observation["yaw"] - plate["yaw"]),
)
angular_cost = abs(angular_delta)
# Plate identity is chosen by yaw, not by the xyz value whose
# error is being measured. Position only breaks near-exact ties.
cost += angular_cost + 0.01 * min(position_cost, 5.0)
pairs.append((observation, plate, position_cost, angular_cost, angular_delta))
if best is None or cost < best[0]:
best = (cost, pairs)
return best[1]
def load_session(distance_m, session_result_path, target_number):
result = load_json(session_result_path)
if not result.get("complete"):
raise ValueError("capture is incomplete: %s" % session_result_path)
start_ns = int(result["motion_segments"][0]["applied_timestamp_ns"])
end_ns = int(result["capture_end_timestamp_ns"])
truth_by_key = {}
for row in iter_jsonl(result["truth"]):
timestamp = int(row["timestamp_ns"])
if start_ns <= timestamp <= end_ns and row.get("has_exact_exposure_truth"):
truth_by_key[(int(row["producer_epoch"]), int(row["frame_seq"]))] = row
samples = []
matched_frames = 0
for observation_row in iter_jsonl(result["observations"]):
timestamp = int(observation_row["timestamp_ns"])
if timestamp < start_ns or timestamp > end_ns:
continue
key = (int(observation_row["producer_epoch"]), int(observation_row["frame_seq"]))
truth_row = truth_by_key.get(key)
if truth_row is None:
continue
valid = []
for armor in observation_row.get("armors", []):
if not armor.get("valid") or int(armor.get("detector_number", -1)) != target_number:
continue
position = np.asarray(armor.get("position_m"), dtype=np.float64)
if position.shape != (3,) or not np.isfinite(position).all():
continue
valid.append({
"index": int(armor["observation_index"]),
"pnp": position,
"yaw": float(armor.get("yaw_absolute_rad", armor.get("yaw_rad", 0.0))),
})
if not valid:
continue
center, plates = tracker_truth_geometry(truth_row, target_number)
pairs = assign_one_to_one(valid, plates)
if not pairs:
continue
matched_frames += 1
for observation, plate, matching_cost, matching_yaw_error, matching_yaw_delta in pairs:
truth_position = plate["tracker"]
error = observation["pnp"] - truth_position
line = truth_position / max(float(np.linalg.norm(truth_position)), 1e-12)
line_of_sight_error = float(np.dot(error, line))
samples.append({
"distance_m": float(distance_m),
"timestamp_ns": timestamp,
"time_s": (timestamp - start_ns) * 1e-9,
"frame_seq": key[1],
"observation_index": observation["index"],
"slot": plate["slot"],
"pnp_x": float(observation["pnp"][0]),
"pnp_y": float(observation["pnp"][1]),
"pnp_z": float(observation["pnp"][2]),
"truth_x": float(truth_position[0]),
"truth_y": float(truth_position[1]),
"truth_z": float(truth_position[2]),
"center_x": float(center[0]),
"center_y": float(center[1]),
"center_z": float(center[2]),
"error_x": float(error[0]),
"error_y": float(error[1]),
"error_z": float(error[2]),
"line_of_sight_error": line_of_sight_error,
"matching_cost": matching_cost,
"matching_yaw_error_rad": matching_yaw_error,
"matching_yaw_delta_rad": matching_yaw_delta,
})
return samples, {
"session_id": result["session_id"],
"distance_m": float(distance_m),
"matched_frames": matched_frames,
"sample_count": len(samples),
"start_timestamp_ns": start_ns,
"end_timestamp_ns": end_ns,
"captured_manifest_sha256": result.get("captured_manifest_sha256"),
"observation_sha256": sha256_file(result["observations"]),
"truth_sha256": sha256_file(result["truth"]),
}
def sha256_file(path):
digest = hashlib.sha256()
with open(path, "rb") as handle:
while True:
block = handle.read(1024 * 1024)
if not block:
break
digest.update(block)
return digest.hexdigest()
def percentiles(values):
array = np.asarray(values, dtype=np.float64)
return {
"mean": float(np.mean(array)),
"p50": float(np.percentile(array, 50)),
"p95": float(np.percentile(array, 95)),
"max": float(np.max(array)),
}
def summarize(samples):
summary = {}
for axis in "xyz":
signed = np.asarray([row["error_" + axis] for row in samples], dtype=np.float64)
summary[axis] = {
"signed_mean_m": float(np.mean(signed)),
"absolute_error_m": percentiles(np.abs(signed)),
}
line = np.asarray([row["line_of_sight_error"] for row in samples], dtype=np.float64)
summary["line_of_sight"] = {
"signed_mean_m": float(np.mean(line)),
"absolute_error_m": percentiles(np.abs(line)),
}
summary["matching_cost_m"] = percentiles([row["matching_cost"] for row in samples])
summary["matching_yaw_error_rad"] = percentiles([
row["matching_yaw_error_rad"] for row in samples
])
return summary
def configure_chinese_font(font_path):
if font_path and os.path.isfile(font_path):
# Matplotlib 2.x does not discover all Windows fonts automatically.
# Register the exact TTF before selecting its family name.
if hasattr(font_manager, "createFontList"):
font_manager.fontManager.ttflist.extend(font_manager.createFontList([font_path]))
font_name = font_manager.FontProperties(fname=font_path).get_name()
plt.rcParams["font.family"] = font_name
plt.rcParams["font.sans-serif"] = [font_name]
plt.rcParams["axes.unicode_minus"] = False
plt.rcParams["svg.fonttype"] = "none"
plt.rcParams["font.size"] = 10
def style_axis(axis):
axis.spines["top"].set_visible(False)
axis.spines["right"].set_visible(False)
axis.grid(axis="y", color="#D1D5DB", linewidth=0.7, alpha=0.65)
axis.set_axisbelow(True)
def plot_axis_error(distances, summaries, output_base):
figure, axes = plt.subplots(1, 2, figsize=(10.8, 4.15), sharex=True)
metric_specs = [("p50", "典型误差（中位数）"), ("p95", "较大误差（95% 分位）")]
labels = {"x": "x（前向，近似深度）", "y": "y（横向）", "z": "z（竖直）"}
markers = {"x": "o", "y": "s", "z": "^"}
for axis_plot, (metric, title) in zip(axes, metric_specs):
for coordinate in "xyz":
values_cm = [
summaries[distance][coordinate]["absolute_error_m"][metric] * 100.0
for distance in distances
]
axis_plot.plot(
distances,
values_cm,
color=AXIS_COLORS[coordinate],
marker=markers[coordinate],
linewidth=2.1,
markersize=5.5,
label=labels[coordinate],
)
axis_plot.set_title(title, fontsize=12, fontweight="bold", pad=10)
axis_plot.set_xlabel("设置距离 / m")
axis_plot.set_ylabel("绝对位置误差 / cm")
axis_plot.set_xticks(distances)
axis_plot.set_ylim(bottom=0.0)
style_axis(axis_plot)
handles, legend_labels = axes[0].get_legend_handles_labels()
figure.legend(handles, legend_labels, loc="upper center", ncol=3, frameon=False,
bbox_to_anchor=(0.5, 1.03))
figure.suptitle("同一转速下，距离如何放大 PnP 的 x / y / z 误差", fontsize=14,
fontweight="bold", y=1.13)
figure.text(0.5, -0.01, "相机、分辨率、目标与转速保持不变；每个点由同曝光真值计算。",
ha="center", color="#4B5563", fontsize=9)
figure.tight_layout(rect=(0, 0.04, 1, 0.96))
for extension in ("png", "svg"):
figure.savefig(output_base + "." + extension, dpi=220, bbox_inches="tight",
facecolor="white")
plt.close(figure)
def split_visibility_segments(samples, max_gap_s=0.25):
segments = []
by_slot = defaultdict(list)
for row in samples:
by_slot[int(row["slot"])].append(row)
for slot_rows in by_slot.values():
slot_rows.sort(key=lambda row: row["time_s"])
current = []
for row in slot_rows:
if current and row["time_s"] - current[-1]["time_s"] > max_gap_s:
if current:
segments.append(current)
current = []
current.append(row)
if current:
segments.append(current)
return segments
def choose_arc_segment(samples):
candidates = []
for segment in split_visibility_segments(samples):
if len(segment) < 25:
continue
truth_xy = np.asarray([
[row["truth_x"] - row["center_x"], row["truth_y"] - row["center_y"]]
for row in segment
])
angle = np.unwrap(np.arctan2(truth_xy[:, 1], truth_xy[:, 0]))
span = float(np.ptp(angle))
if span < math.radians(12.0):
continue
candidates.append((span * math.sqrt(len(segment)), span, segment))
if not candidates:
raise ValueError("no continuous armor arc contains enough samples")
candidates.sort(key=lambda item: item[0], reverse=True)
return candidates[0][2], candidates[0][1]
def fit_phase_harmonic_locus(segment):
truth_xy = np.asarray([
[row["truth_x"] - row["center_x"], row["truth_y"] - row["center_y"]]
for row in segment
])
observed_xy = np.asarray([
[row["pnp_x"] - row["center_x"], row["pnp_y"] - row["center_y"]]
for row in segment
])
theta = np.unwrap(np.arctan2(truth_xy[:, 1], truth_xy[:, 0]))
design = np.column_stack([np.ones_like(theta), np.cos(theta), np.sin(theta)])
inlier_mask = np.ones(len(theta), dtype=bool)
coefficients = None
for _ in range(4):
coefficients, _, _, _ = np.linalg.lstsq(
design[inlier_mask], observed_xy[inlier_mask], rcond=None
)
residual = np.linalg.norm(observed_xy - np.dot(design, coefficients), axis=1)
threshold = float(np.percentile(residual[inlier_mask], 90))
next_mask = residual <= threshold
if int(np.sum(next_mask)) < 25 or np.array_equal(next_mask, inlier_mask):
break
inlier_mask = next_mask
offset = coefficients[0]
transform = np.column_stack([coefficients[1], coefficients[2]])
principal_scales = np.linalg.svd(transform, compute_uv=False)
fitted = np.dot(design, coefficients)
return {
"truth_xy": truth_xy,
"observed_xy": observed_xy,
"theta": theta,
"coefficients": coefficients,
"offset": offset,
"transform": transform,
"principal_scales": principal_scales,
"fitted": fitted,
"inlier_mask": inlier_mask,
"fit_rms_m": float(np.sqrt(np.mean(
np.sum((observed_xy[inlier_mask] - fitted[inlier_mask]) ** 2, axis=1)
))),
}
def plot_trajectory_model(segment, fit, distance_m, angular_span, output_base):
truth_xy = fit["truth_xy"]
observed_xy = fit["observed_xy"]
radius = float(np.median(np.linalg.norm(truth_xy, axis=1)))
full_theta = np.linspace(0.0, 2.0 * math.pi, 360)
ideal_circle = np.column_stack([radius * np.cos(full_theta), radius * np.sin(full_theta)])
inlier_mask = fit["inlier_mask"]
all_points = np.vstack([ideal_circle, observed_xy[inlier_mask], fit["fitted"][inlier_mask]])
low = np.percentile(all_points, 1, axis=0)
high = np.percentile(all_points, 99, axis=0)
center = (low + high) * 0.5
half_span = max(float(np.max(high - low)) * 0.62, radius * 1.25)
# A vertical comparison keeps labels readable inside the tutorial's narrow
# article column while preserving an identical scale in both panels.
figure, axes = plt.subplots(2, 1, figsize=(7.8, 8.8), sharex=True, sharey=True)
axes[0].plot(ideal_circle[:, 0], ideal_circle[:, 1], color="#9CA3AF", linewidth=1.2,
linestyle="--", label="装甲板完整理论圆")
axes[0].plot(truth_xy[:, 0], truth_xy[:, 1], color="#0F4D92", linewidth=2.8,
label="本段曝光真值")
axes[0].scatter([0], [0], marker="*", s=90, color="#111827", label="目标中心", zorder=5)
axes[0].set_title("真值：同一块装甲板沿圆弧运动", fontsize=12, fontweight="bold")
axes[1].plot(ideal_circle[:, 0], ideal_circle[:, 1], color="#9CA3AF", linewidth=1.2,
linestyle="--", label="理论圆")
axes[1].scatter(observed_xy[~inlier_mask, 0], observed_xy[~inlier_mask, 1], s=12,
color="#B8BDC6", alpha=0.35, edgecolors="none",
label="离群观测（仍计入误差统计）")
axes[1].scatter(observed_xy[inlier_mask, 0], observed_xy[inlier_mask, 1], s=15,
color="#B64342", alpha=0.74, edgecolors="none", label="PnP 观测")
axes[1].plot(fit["fitted"][:, 0], fit["fitted"][:, 1], color="#D97706",
linewidth=2.8, linestyle="-.", label="一阶相位谐波拟合")
axes[1].scatter([0], [0], marker="*", s=90, color="#111827", zorder=5)
axes[1].set_title("PnP：观测轨迹与候选相位模型", fontsize=12, fontweight="bold")
for axis in axes:
axis.set_aspect("equal", adjustable="box")
axis.set_xlim(center[0] - half_span, center[0] + half_span)
axis.set_ylim(center[1] - half_span, center[1] + half_span)
axis.set_xlabel("相对目标中心 x / m（前向、近似深度）")
axis.set_ylabel("相对目标中心 y / m（横向）")
axis.grid(color="#E5E7EB", linewidth=0.7)
axis.spines["top"].set_visible(False)
axis.spines["right"].set_visible(False)
axis.legend(loc="best", frameon=False, fontsize=8.5)
axes[0].set_xlabel("")
ratio = float(
fit["principal_scales"][0] / max(fit["principal_scales"][1], 1e-12)
)
figure.suptitle("真值圆弧与 PnP 观测轨迹的候选模型对照", fontsize=14,
fontweight="bold", y=0.995)
figure.text(
0.5,
0.005,
"设置距离 %.0f m；连续可见角度 %.1f°；谐波主尺度比 %.2f（仅描述本段，不判定轨迹类型）。" % (
distance_m, math.degrees(angular_span), ratio,
),
ha="center",
fontsize=9,
color="#4B5563",
)
figure.tight_layout(rect=(0, 0.05, 1, 0.95))
for extension in ("png", "svg"):
figure.savefig(output_base + "." + extension, dpi=220, bbox_inches="tight",
facecolor="white")
plt.close(figure)
def write_samples(path, samples):
fieldnames = [
"distance_m", "timestamp_ns", "time_s", "frame_seq", "observation_index", "slot",
"pnp_x", "pnp_y", "pnp_z", "truth_x", "truth_y", "truth_z",
"center_x", "center_y", "center_z", "error_x", "error_y", "error_z",
"line_of_sight_error", "matching_cost",
"matching_yaw_error_rad",
"matching_yaw_delta_rad",
]
with open(path, "w", newline="", encoding="utf-8") as handle:
writer = csv.DictWriter(handle, fieldnames=fieldnames)
writer.writeheader()
writer.writerows(samples)
def main():
parser = argparse.ArgumentParser()
parser.add_argument("--session", action="append", required=True,
help="DISTANCE=path/to/session_result.json")
parser.add_argument("--output-dir", required=True)
parser.add_argument("--image-dir", required=True)
parser.add_argument("--arc-distance", type=float, default=6.0)
parser.add_argument("--target-number", type=int, default=3)
parser.add_argument("--font", default=r"C:\Windows\Fonts\simhei.ttf")
args = parser.parse_args()
os.makedirs(args.output_dir, exist_ok=True)
os.makedirs(args.image_dir, exist_ok=True)
configure_chinese_font(args.font)
sessions = []
for item in args.session:
distance_text, separator, path = item.partition("=")
if not separator:
parser.error("invalid --session: %s" % item)
sessions.append((float(distance_text), os.path.abspath(path)))
sessions.sort(key=lambda item: item[0])
all_samples = []
capture = {}
summaries = {}
samples_by_distance = {}
for distance, path in sessions:
samples, capture_summary = load_session(distance, path, args.target_number)
if not samples:
raise ValueError("session at %.1f m produced no matched samples" % distance)
all_samples.extend(samples)
samples_by_distance[distance] = samples
capture[distance] = capture_summary
summaries[distance] = summarize(samples)
distances = [item[0] for item in sessions]
plot_axis_error(
distances,
summaries,
os.path.join(args.image_dir, "pnp-coordinate-error-by-distance"),
)
if args.arc_distance not in samples_by_distance:
raise ValueError("arc distance %.1f is not one of the sessions" % args.arc_distance)
segment, angular_span = choose_arc_segment(samples_by_distance[args.arc_distance])
fit = fit_phase_harmonic_locus(segment)
plot_trajectory_model(
segment,
fit,
args.arc_distance,
angular_span,
os.path.join(args.image_dir, "pnp-observed-trajectory-model"),
)
write_samples(os.path.join(args.output_dir, "matched-samples.csv"), all_samples)
serializable_summaries = {str(distance): summaries[distance] for distance in distances}
serializable_capture = {str(distance): capture[distance] for distance in distances}
arc_summary = {
"distance_m": args.arc_distance,
"slot": int(segment[0]["slot"]),
"sample_count": len(segment),
"time_start_s": float(segment[0]["time_s"]),
"time_end_s": float(segment[-1]["time_s"]),
"truth_angular_span_deg": math.degrees(angular_span),
"truth_radius_m": float(np.median(np.linalg.norm(fit["truth_xy"], axis=1))),
"phase_harmonic_principal_scales_m": [
float(value) for value in fit["principal_scales"]
],
"phase_harmonic_scale_ratio": float(
fit["principal_scales"][0]
/ max(fit["principal_scales"][1], 1e-12)
),
"model_boundary": (
"oracle truth phase, descriptive fit for this visible segment only; "
"it does not classify the trajectory as a circle or ellipse"
),
"fit_rms_m": fit["fit_rms_m"],
"fit_inlier_count": int(np.sum(fit["inlier_mask"])),
}
output = {
"schema_version": "pnp-coordinate-error-teaching-experiment-v1",
"coordinate_frame": "tracker/chassis: x forward, y lateral, z vertical",
"target_number": args.target_number,
"capture": serializable_capture,
"axis_error": serializable_summaries,
"arc": arc_summary,
}
with open(os.path.join(args.output_dir, "summary.json"), "w", encoding="utf-8") as handle:
json.dump(output, handle, ensure_ascii=False, indent=2, sort_keys=True)
handle.write("\n")
print(json.dumps(output, ensure_ascii=False, indent=2, sort_keys=True))
if __name__ == "__main__":
main()
build_noise_distributions.py
#!/usr/bin/env python3
"""Build teaching figures from detector corners to PnP observation noise.
The script joins every observation with the exact simulator truth from the same
exposure. Truth is used only for offline evaluation. It projects the physical
armor corners through the recorded camera model, orders them with the same
screen convention as the detector, and then measures both pixel-space corner
residuals and tracker-frame PnP position residuals.
"""
from __future__ import annotations
import argparse
import csv
import hashlib
import itertools
import json
import math
import os
from collections import defaultdict
import matplotlib
matplotlib.use("Agg")
import matplotlib.font_manager as font_manager # noqa: E402
import matplotlib.gridspec as gridspec # noqa: E402
import matplotlib.pyplot as plt # noqa: E402
from matplotlib.patches import Ellipse # noqa: E402
import numpy as np # noqa: E402
from scipy.stats import norm as normal_distribution # noqa: E402
ARMOR_WIDTH_M = 0.135
ARMOR_HEIGHT_M = 0.055
KNOWN_PITCH_RAD = math.radians(15.0)
CORNER_NAMES = ("左下", "左上", "右上", "右下")
DISTANCE_COLORS = {2.0: "#0F4D92", 4.0: "#D97706", 6.0: "#B64342"}
AXIS_COLORS = {"x": "#0F4D92", "y": "#D97706", "z": "#2E8B57"}
def load_json(path: str) -> dict:
with open(path, "r", encoding="utf-8-sig") as handle:
return json.load(handle)
def iter_jsonl(path: str):
with open(path, "r", encoding="utf-8") as handle:
for line in handle:
if line.strip():
yield json.loads(line)
def sha256_file(path: str) -> str:
digest = hashlib.sha256()
with open(path, "rb") as handle:
for block in iter(lambda: handle.read(1024 * 1024), b""):
digest.update(block)
return digest.hexdigest()
def quaternion_rotation(values) -> np.ndarray:
quaternion = np.asarray(values, dtype=np.float64)
if quaternion.shape != (4,) or not np.isfinite(quaternion).all():
raise ValueError("quaternion must contain four finite values")
norm = float(np.linalg.norm(quaternion))
if norm <= 1e-12:
raise ValueError("quaternion norm is zero")
w, x, y, z = quaternion / norm
return np.asarray([
[1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - z * w), 2.0 * (x * z + y * w)],
[2.0 * (x * y + z * w), 1.0 - 2.0 * (x * x + z * z), 2.0 * (y * z - x * w)],
[2.0 * (x * z - y * w), 2.0 * (y * z + x * w), 1.0 - 2.0 * (x * x + y * y)],
], dtype=np.float64)
def tracker_to_world_rotation(observation: dict) -> np.ndarray:
world_from_gimbal = quaternion_rotation(
observation["tracker_gimbal_quaternion_world_wxyz"]
)
yaw = -math.radians(float(observation["gimbal_yaw_deg"]))
pitch = math.radians(float(observation["gimbal_pitch_deg"]))
rotation_z = np.asarray([
[math.cos(yaw), -math.sin(yaw), 0.0],
[math.sin(yaw), math.cos(yaw), 0.0],
[0.0, 0.0, 1.0],
])
rotation_y = np.asarray([
[math.cos(pitch), 0.0, math.sin(pitch)],
[0.0, 1.0, 0.0],
[-math.sin(pitch), 0.0, math.cos(pitch)],
])
result = world_from_gimbal @ (rotation_z @ rotation_y).T
if not np.allclose(result.T @ result, np.eye(3), atol=1e-7, rtol=0.0):
raise ValueError("tracker rotation is not orthonormal")
return result
def camera_to_tracker_rotation(observation: dict) -> np.ndarray:
yaw = math.radians(float(observation["gimbal_yaw_deg"]))
pitch = -math.radians(float(observation["gimbal_pitch_deg"]))
yaw_rotation = np.asarray([
[math.cos(yaw), 0.0, math.sin(yaw)],
[0.0, 1.0, 0.0],
[-math.sin(yaw), 0.0, math.cos(yaw)],
])
pitch_rotation = np.asarray([
[1.0, 0.0, 0.0],
[0.0, math.cos(pitch), -math.sin(pitch)],
[0.0, math.sin(pitch), math.cos(pitch)],
])
camera_convention = np.asarray([
[0.0, 0.0, 1.0],
[-1.0, 0.0, 0.0],
[0.0, -1.0, 0.0],
])
return camera_convention @ yaw_rotation @ pitch_rotation
def camera_from_tracker_transform(observation: dict) -> tuple[np.ndarray, np.ndarray]:
camera_to_tracker = camera_to_tracker_rotation(observation)
camera_to_gimbal = np.asarray(
observation["R_camera2gimbal"], dtype=np.float64
).reshape(3, 3)
gimbal_to_tracker = camera_to_tracker @ camera_to_gimbal.T
camera_origin_tracker = gimbal_to_tracker @ np.asarray(
observation["t_camera2gimbal_m"], dtype=np.float64
)
return camera_to_tracker.T, -camera_to_tracker.T @ camera_origin_tracker
def selected_target(truth: dict, target_number: int) -> dict:
if not truth.get("has_exact_exposure_truth"):
raise ValueError("truth row has no exact exposure truth")
selected_id = int(truth["selected_target_id"])
candidates = [
target for target in truth["ground_truth"]["targets"]
if int(target.get("target_id", -1)) == selected_id
and int(target.get("armor_label", -1)) == target_number
]
if len(candidates) != 1:
raise ValueError("expected exactly one selected target")
return candidates[0]
def world_geometry(truth: dict, target_number: int) -> tuple[np.ndarray, dict[int, dict]]:
target = selected_target(truth, target_number)
target_center = np.asarray(target["world_position_m"], dtype=np.float64)
target_rotation = quaternion_rotation(target["world_quaternion_wxyz"])
plates = {}
for armor in target["armors"]:
slot = int(armor["relative_slot"])
position = target_center + target_rotation @ np.asarray(
armor["chassis_local_position_m"], dtype=np.float64
)
normal = target_rotation @ np.asarray(
armor["chassis_local_outward_normal"], dtype=np.float64
)
plates[slot] = {"position_world": position, "normal_world": normal}
if set(plates) != set(range(4)):
raise ValueError("truth row does not contain four armor slots")
return target_center, plates
def distort(points: np.ndarray, coefficients_value) -> np.ndarray:
coefficients = np.asarray(coefficients_value, dtype=np.float64)
if coefficients.shape not in {(0,), (4,), (5,), (8,)}:
raise ValueError("distortion vector must have 0, 4, 5, or 8 values")
if coefficients.size == 0:
return points
k1, k2, p1, p2, k3, k4, k5, k6 = np.pad(
coefficients, (0, 8 - coefficients.size), mode="constant"
)
x_value = points[:, 0]
y_value = points[:, 1]
radius2 = x_value * x_value + y_value * y_value
radius4 = radius2 * radius2
radius6 = radius4 * radius2
denominator = 1.0 + k4 * radius2 + k5 * radius4 + k6 * radius6
radial = (1.0 + k1 * radius2 + k2 * radius4 + k3 * radius6) / denominator
cross = x_value * y_value
return np.column_stack((
x_value * radial + 2.0 * p1 * cross + p2 * (radius2 + 2.0 * x_value * x_value),
y_value * radial + p1 * (radius2 + 2.0 * y_value * y_value) + 2.0 * p2 * cross,
))
def camera_points_to_pixels(observation: dict, camera_points: np.ndarray) -> np.ndarray:
if np.any(camera_points[:, 2] <= 0.0):
raise ValueError("camera projection contains nonpositive depth")
normalized = camera_points[:, :2] / camera_points[:, 2:3]
distorted = distort(normalized, observation["distortion_coeffs"])
homogeneous = np.column_stack((distorted, np.ones(len(distorted))))
camera_matrix = np.asarray(
observation["camera_matrix"], dtype=np.float64
).reshape(3, 3)
pixel_h = homogeneous @ camera_matrix.T
return pixel_h[:, :2] / pixel_h[:, 2:3]
def rotation_z(angle: float) -> np.ndarray:
return np.asarray([
[math.cos(angle), -math.sin(angle), 0.0],
[math.sin(angle), math.cos(angle), 0.0],
[0.0, 0.0, 1.0],
])
def rotation_y(angle: float) -> np.ndarray:
return np.asarray([
[math.cos(angle), 0.0, math.sin(angle)],
[0.0, 1.0, 0.0],
[-math.sin(angle), 0.0, math.cos(angle)],
])
def object_corners() -> np.ndarray:
half_width = ARMOR_WIDTH_M / 2.0
half_height = ARMOR_HEIGHT_M / 2.0
return np.asarray([
[0.0, half_width, -half_height],
[0.0, half_width, half_height],
[0.0, -half_width, half_height],
[0.0, -half_width, -half_height],
])
def canonicalize_screen_corners(points: np.ndarray) -> np.ndarray:
"""Return detector order: bottom-left, top-left, top-right, bottom-right."""
points = np.asarray(points, dtype=np.float64)
by_x = np.argsort(points[:, 0], kind="stable")
if points[by_x[2], 0] <= points[by_x[1], 0]:
raise ValueError("projected left/right pairs are degenerate")
left = by_x[:2][np.argsort(points[by_x[:2], 1], kind="stable")]
right = by_x[2:][np.argsort(points[by_x[2:], 1], kind="stable")]
if points[left[1], 1] <= points[left[0], 1] or points[right[1], 1] <= points[right[0], 1]:
raise ValueError("projected top/bottom pairs are degenerate")
return points[np.asarray((left[1], left[0], right[0], right[1]))]
def projected_truth(observation: dict, truth: dict, target_number: int) -> tuple[np.ndarray, dict]:
target_center_world, world_plates = world_geometry(truth, target_number)
tracker_origin_world = np.asarray(
observation["tracker_origin_world_ros_m"], dtype=np.float64
)
world_from_tracker = tracker_to_world_rotation(observation)
camera_from_tracker, camera_translation = camera_from_tracker_transform(observation)
target_center_tracker = (target_center_world - tracker_origin_world) @ world_from_tracker
plates = {}
for slot, plate in world_plates.items():
center_tracker = (plate["position_world"] - tracker_origin_world) @ world_from_tracker
normal_tracker = plate["normal_world"] @ world_from_tracker
outward_yaw = math.atan2(float(normal_tracker[1]), float(normal_tracker[0]))
plate_rotation = rotation_z(outward_yaw + math.pi) @ rotation_y(KNOWN_PITCH_RAD)
corners_tracker = center_tracker[None, :] + object_corners() @ plate_rotation.T
corners_camera = corners_tracker @ camera_from_tracker.T + camera_translation[None, :]
center_camera = camera_from_tracker @ center_tracker + camera_translation
corners_pixel = canonicalize_screen_corners(
camera_points_to_pixels(observation, corners_camera)
)
center_pixel = camera_points_to_pixels(observation, center_camera[None, :])[0]
plates[slot] = {
"center_tracker": center_tracker,
"center_pixel": center_pixel,
"corners_pixel": corners_pixel,
"range_m": float(np.linalg.norm(center_tracker)),
}
return target_center_tracker, plates
def polygon_area(points: np.ndarray) -> float:
following = np.roll(points, -1, axis=0)
return abs(float(0.5 * np.sum(
points[:, 0] * following[:, 1] - points[:, 1] * following[:, 0]
)))
def plate_width(points: np.ndarray) -> float:
return float(0.5 * (
np.linalg.norm(points[2] - points[1]) + np.linalg.norm(points[3] - points[0])
))
def assign_by_pixel_center(observations: list[dict], truth_plates: dict) -> list[tuple[dict, int]]:
slots = sorted(truth_plates)
if len(observations) > len(slots):
observations = observations[:len(slots)]
best = None
for selected_slots in itertools.permutations(slots, len(observations)):
distances = [
float(np.linalg.norm(observation["center_pixel"] - truth_plates[slot]["center_pixel"]))
for observation, slot in zip(observations, selected_slots)
]
cost = float(sum(distances))
if best is None or cost < best[0]:
best = (cost, list(zip(observations, selected_slots)), distances)
if best is None:
return []
for pair, distance in zip(best[1], best[2]):
pair[0]["association_distance_px"] = distance
return best[1]
def load_session(distance_m: float, session_result_path: str, target_number: int):
result = load_json(session_result_path)
if not result.get("complete"):
raise ValueError("capture is incomplete: %s" % session_result_path)
start_ns = int(result["motion_segments"][0]["applied_timestamp_ns"])
end_ns = int(result["capture_end_timestamp_ns"])
truth_by_key = {}
for truth in iter_jsonl(result["truth"]):
timestamp_ns = int(truth["timestamp_ns"])
if start_ns <= timestamp_ns <= end_ns and truth.get("has_exact_exposure_truth"):
key = (int(truth["producer_epoch"]), int(truth["frame_seq"]), timestamp_ns)
truth_by_key[key] = truth
observations_out = []
corners_out = []
stream_frames = 0
matched_frames = 0
rejected = defaultdict(int)
for frame in iter_jsonl(result["observations"]):
timestamp_ns = int(frame["timestamp_ns"])
if not start_ns <= timestamp_ns <= end_ns:
continue
stream_frames += 1
key = (int(frame["producer_epoch"]), int(frame["frame_seq"]), timestamp_ns)
truth = truth_by_key.get(key)
if truth is None:
rejected["missing_exact_truth"] += 1
continue
candidates = []
for armor in frame.get("armors", []):
if not armor.get("valid") or int(armor.get("detector_number", -1)) != target_number:
continue
raw = np.asarray(armor.get("raw_corners_px"), dtype=np.float64)
refined = np.asarray(armor.get("refined_corners_px"), dtype=np.float64)
position = np.asarray(armor.get("position_m"), dtype=np.float64)
if raw.shape != (4, 2) or refined.shape != (4, 2) or position.shape != (3,):
rejected["incomplete_observation"] += 1
continue
if not np.isfinite(raw).all() or not np.isfinite(refined).all() or not np.isfinite(position).all():
rejected["nonfinite_observation"] += 1
continue
candidates.append({
"observation_index": int(armor["observation_index"]),
"raw": raw,
"refined": refined,
"pnp": position,
"center_pixel": refined.mean(axis=0),
})
if not candidates:
rejected["no_target_detection"] += 1
continue
try:
target_center, truth_plates = projected_truth(frame, truth, target_number)
pairs = assign_by_pixel_center(candidates, truth_plates)
except ValueError:
rejected["projection_or_order_failure"] += 1
continue
accepted_in_frame = 0
for observation, slot in pairs:
truth_plate = truth_plates[slot]
if observation["association_distance_px"] > 40.0:
rejected["association_over_40px"] += 1
continue
truth_corners = truth_plate["corners_pixel"]
raw_residual = observation["raw"] - truth_corners
refined_residual = observation["refined"] - truth_corners
position_error = observation["pnp"] - truth_plate["center_tracker"]
line = truth_plate["center_tracker"] / max(truth_plate["range_m"], 1e-12)
line_of_sight_error = float(position_error @ line)
truth_area = polygon_area(truth_corners)
raw_area = polygon_area(observation["raw"])
refined_area = polygon_area(observation["refined"])
observation_key = "%s:%d:%d" % (
result["session_id"], int(frame["frame_seq"]), observation["observation_index"]
)
common = {
"observation_key": observation_key,
"session_id": result["session_id"],
"distance_m": float(distance_m),
"timestamp_ns": timestamp_ns,
"time_s": (timestamp_ns - start_ns) * 1e-9,
"frame_seq": int(frame["frame_seq"]),
"observation_index": observation["observation_index"],
"slot": int(slot),
"association_distance_px": observation["association_distance_px"],
}
row = dict(common)
row.update({
"pnp_x": float(observation["pnp"][0]),
"pnp_y": float(observation["pnp"][1]),
"pnp_z": float(observation["pnp"][2]),
"truth_x": float(truth_plate["center_tracker"][0]),
"truth_y": float(truth_plate["center_tracker"][1]),
"truth_z": float(truth_plate["center_tracker"][2]),
"target_center_x": float(target_center[0]),
"target_center_y": float(target_center[1]),
"target_center_z": float(target_center[2]),
"error_x": float(position_error[0]),
"error_y": float(position_error[1]),
"error_z": float(position_error[2]),
"line_of_sight_error": line_of_sight_error,
"truth_range_m": truth_plate["range_m"],
"truth_width_px": plate_width(truth_corners),
"raw_width_px": plate_width(observation["raw"]),
"refined_width_px": plate_width(observation["refined"]),
"raw_scale_error": math.sqrt(raw_area / truth_area) - 1.0,
"refined_scale_error": math.sqrt(refined_area / truth_area) - 1.0,
"relative_los_error": line_of_sight_error / truth_plate["range_m"],
"raw_corner_rms_px": float(np.sqrt(np.mean(np.sum(raw_residual ** 2, axis=1)))),
"refined_corner_rms_px": float(np.sqrt(np.mean(np.sum(refined_residual ** 2, axis=1)))),
})
observations_out.append(row)
for corner_index, corner_name in enumerate(CORNER_NAMES):
corner_row = dict(common)
corner_row.update({
"corner_index": corner_index,
"corner_name": corner_name,
"truth_u_px": float(truth_corners[corner_index, 0]),
"truth_v_px": float(truth_corners[corner_index, 1]),
"raw_u_px": float(observation["raw"][corner_index, 0]),
"raw_v_px": float(observation["raw"][corner_index, 1]),
"refined_u_px": float(observation["refined"][corner_index, 0]),
"refined_v_px": float(observation["refined"][corner_index, 1]),
"raw_du_px": float(raw_residual[corner_index, 0]),
"raw_dv_px": float(raw_residual[corner_index, 1]),
"raw_error_px": float(np.linalg.norm(raw_residual[corner_index])),
"refined_du_px": float(refined_residual[corner_index, 0]),
"refined_dv_px": float(refined_residual[corner_index, 1]),
"refined_error_px": float(np.linalg.norm(refined_residual[corner_index])),
})
corners_out.append(corner_row)
accepted_in_frame += 1
if accepted_in_frame:
matched_frames += 1
capture = {
"session_id": result["session_id"],
"distance_m": float(distance_m),
"stream_frame_count": stream_frames,
"matched_frame_count": matched_frames,
"matched_observation_count": len(observations_out),
"corner_sample_count": len(corners_out),
"rejected_frame_or_detection_counts": dict(sorted(rejected.items())),
"observation_sha256": sha256_file(result["observations"]),
"truth_sha256": sha256_file(result["truth"]),
"captured_manifest_sha256": result.get("captured_manifest_sha256"),
}
return observations_out, corners_out, capture
def describe(values) -> dict:
array = np.asarray(values, dtype=np.float64)
array = array[np.isfinite(array)]
if array.size == 0:
return {"count": 0}
mean = float(np.mean(array))
standard_deviation = float(np.std(array, ddof=1)) if array.size > 1 else 0.0
centered = array - mean
if standard_deviation > 0.0:
skewness = float(np.mean((centered / standard_deviation) ** 3))
excess_kurtosis = float(np.mean((centered / standard_deviation) ** 4) - 3.0)
else:
skewness = 0.0
excess_kurtosis = 0.0
median = float(np.median(array))
return {
"count": int(array.size),
"mean": mean,
"standard_deviation": standard_deviation,
"median": median,
"mad": float(np.median(np.abs(array - median))),
"p01": float(np.percentile(array, 1)),
"p05": float(np.percentile(array, 5)),
"p95": float(np.percentile(array, 95)),
"p99": float(np.percentile(array, 99)),
"minimum": float(np.min(array)),
"maximum": float(np.max(array)),
"skewness": skewness,
"excess_kurtosis": excess_kurtosis,
}
def lagged_autocorrelation(rows: list[dict], field: str, max_lag: int = 20):
sequences = []
grouped = defaultdict(list)
for row in rows:
grouped[(row["distance_m"], row["slot"])].append(row)
for values in grouped.values():
values.sort(key=lambda item: item["timestamp_ns"])
current = []
for row in values:
if current and row["time_s"] - current[-1]["time_s"] > 0.08:
if len(current) >= max_lag + 3:
sequences.append(current)
current = []
current.append(row)
if len(current) >= max_lag + 3:
sequences.append(current)
result = []
for lag in range(max_lag + 1):
left_values = []
right_values = []
for sequence in sequences:
values = np.asarray([row[field] for row in sequence], dtype=np.float64)
values -= np.mean(values)
if lag == 0:
left_values.append(values)
right_values.append(values)
else:
left_values.append(values[:-lag])
right_values.append(values[lag:])
if not left_values:
result.append(float("nan"))
continue
left = np.concatenate(left_values)
right = np.concatenate(right_values)
denominator = float(np.sqrt(np.sum(left ** 2) * np.sum(right ** 2)))
result.append(float(np.sum(left * right) / denominator) if denominator > 0 else 0.0)
return result, len(sequences)
def summarize(observation_rows: list[dict], corner_rows: list[dict], capture: dict) -> dict:
result = {"capture": capture, "by_distance": {}}
distances = sorted({row["distance_m"] for row in observation_rows})
for distance in distances:
observations = [row for row in observation_rows if row["distance_m"] == distance]
corners = [row for row in corner_rows if row["distance_m"] == distance]
raw_vectors = np.asarray([[row["raw_du_px"], row["raw_dv_px"]] for row in corners])
refined_vectors = np.asarray([
[row["refined_du_px"], row["refined_dv_px"]] for row in corners
])
position = {}
for axis in "xyz":
signed = np.asarray([row["error_" + axis] for row in observations])
position[axis] = {
"signed_m": describe(signed),
"absolute_m": describe(np.abs(signed)),
}
position_vectors = np.asarray([
[row["error_x"], row["error_y"], row["error_z"]]
for row in observations
])
raw_by_corner = {}
for corner_name in CORNER_NAMES:
selected = [row for row in corners if row["corner_name"] == corner_name]
raw_by_corner[corner_name] = {
"du_px": describe([row["raw_du_px"] for row in selected]),
"dv_px": describe([row["raw_dv_px"] for row in selected]),
"radial_px": describe([row["raw_error_px"] for row in selected]),
}
acf, segment_count = lagged_autocorrelation(
observations, "line_of_sight_error", max_lag=20
)
scale = np.asarray([row["refined_scale_error"] for row in observations])
relative_depth = np.asarray([row["relative_los_error"] for row in observations])
result["by_distance"][str(distance)] = {
"observation_count": len(observations),
"corner_count": len(corners),
"raw_corner": {
"du_px": describe(raw_vectors[:, 0]),
"dv_px": describe(raw_vectors[:, 1]),
"radial_px": describe(np.linalg.norm(raw_vectors, axis=1)),
"covariance_px2": np.cov(raw_vectors.T).tolist(),
"du_dv_correlation": float(np.corrcoef(raw_vectors.T)[0, 1]),
},
"refined_corner": {
"du_px": describe(refined_vectors[:, 0]),
"dv_px": describe(refined_vectors[:, 1]),
"radial_px": describe(np.linalg.norm(refined_vectors, axis=1)),
"covariance_px2": np.cov(refined_vectors.T).tolist(),
"du_dv_correlation": float(np.corrcoef(refined_vectors.T)[0, 1]),
},
"pnp_position": position,
"pnp_position_covariance_m2": np.cov(position_vectors.T).tolist(),
"pnp_position_correlation": np.corrcoef(position_vectors.T).tolist(),
"raw_corner_by_name": raw_by_corner,
"line_of_sight": {
"signed_m": describe([row["line_of_sight_error"] for row in observations]),
"absolute_m": describe([abs(row["line_of_sight_error"]) for row in observations]),
"acf_lag_0_to_20": acf,
"continuous_segment_count": segment_count,
},
"propagation": {
"refined_scale_vs_relative_los_correlation": float(
np.corrcoef(scale, relative_depth)[0, 1]
),
"refined_scale_error": describe(scale),
"relative_los_error": describe(relative_depth),
},
}
result["overall"] = {
"matched_observation_count": len(observation_rows),
"corner_sample_count": len(corner_rows),
"association_distance_px": describe([
row["association_distance_px"] for row in observation_rows
]),
}
return result
def configure_chinese_font(font_path: str) -> None:
if font_path and os.path.isfile(font_path):
if hasattr(font_manager.fontManager, "addfont"):
font_manager.fontManager.addfont(font_path)
elif hasattr(font_manager, "createFontList"):
font_manager.fontManager.ttflist.extend(font_manager.createFontList([font_path]))
font_name = font_manager.FontProperties(fname=font_path).get_name()
plt.rcParams["font.family"] = font_name
plt.rcParams["font.sans-serif"] = [font_name]
plt.rcParams["axes.unicode_minus"] = False
plt.rcParams["svg.fonttype"] = "none"
plt.rcParams["font.size"] = 10
def style_axis(axis, grid_axis="both") -> None:
axis.spines["top"].set_visible(False)
axis.spines["right"].set_visible(False)
axis.grid(axis=grid_axis, color="#E5E7EB", linewidth=0.7, alpha=0.85)
axis.set_axisbelow(True)
def add_covariance_ellipse(axis, values: np.ndarray, color: str, label: str) -> None:
covariance = np.cov(values.T)
eigenvalues, eigenvectors = np.linalg.eigh(covariance)
order = np.argsort(eigenvalues)[::-1]
eigenvalues = eigenvalues[order]
eigenvectors = eigenvectors[:, order]
angle = math.degrees(math.atan2(eigenvectors[1, 0], eigenvectors[0, 0]))
# sqrt(chi2.ppf(0.95, 2)) = 2.4477
scale = 2.4477
ellipse = Ellipse(
xy=np.mean(values, axis=0),
width=2.0 * scale * math.sqrt(max(eigenvalues[0], 0.0)),
height=2.0 * scale * math.sqrt(max(eigenvalues[1], 0.0)),
angle=angle,
fill=False,
color=color,
linewidth=2.0,
label=label,
)
axis.add_patch(ellipse)
def save_figure(figure, output_base: str) -> None:
for extension in ("png", "svg"):
output_path = output_base + "." + extension
figure.savefig(
output_path,
dpi=230,
bbox_inches="tight",
facecolor="white",
)
if extension == "svg":
# Matplotlib 2.x leaves spaces at SVG path line endings. Normalize
# them so the editable source also passes repository whitespace checks.
with open(output_path, "r", encoding="utf-8") as handle:
normalized = "\n".join(line.rstrip() for line in handle.read().splitlines()) + "\n"
with open(output_path, "w", encoding="utf-8", newline="\n") as handle:
handle.write(normalized)
plt.close(figure)
def plot_corner_distribution(corner_rows: list[dict], image_dir: str) -> None:
distances = sorted({row["distance_m"] for row in corner_rows})
four_meter = [row for row in corner_rows if row["distance_m"] == 4.0]
by_observation = defaultdict(list)
for row in four_meter:
by_observation[row["observation_key"]].append(row)
candidates = []
for rows in by_observation.values():
if len(rows) == 4:
rms = math.sqrt(sum(row["raw_error_px"] ** 2 for row in rows) / 4.0)
candidates.append((rms, rows))
candidates.sort(key=lambda item: item[0])
example = sorted(candidates[len(candidates) // 2][1], key=lambda row: row["corner_index"])
truth = np.asarray([[row["truth_u_px"], row["truth_v_px"]] for row in example])
raw = np.asarray([[row["raw_u_px"], row["raw_v_px"]] for row in example])
figure = plt.figure(figsize=(8.2, 12.0))
grid = gridspec.GridSpec(3, 1, height_ratios=(1.05, 1.15, 1.0), hspace=0.42)
axis_example = figure.add_subplot(grid[0])
closed = [0, 1, 2, 3, 0]
axis_example.plot(truth[closed, 0], truth[closed, 1], color="#0F4D92", linewidth=2.5,
marker="o", label="同曝光真值角点")
axis_example.plot(raw[closed, 0], raw[closed, 1], color="#B64342", linewidth=2.0,
marker="s", linestyle="--", label="检测器原始角点")
arrow_scale = 4.0
for index, name in enumerate(CORNER_NAMES):
delta = (raw[index] - truth[index]) * arrow_scale
axis_example.annotate(
"", xy=truth[index] + delta, xytext=truth[index],
arrowprops={"arrowstyle": "->", "color": "#D97706", "lw": 1.8},
)
horizontal = "left" if index in (0, 1) else "right"
vertical_offset = -0.7 if index in (0, 3) else 0.7
vertical = "bottom" if index in (0, 3) else "top"
axis_example.text(
truth[index, 0] + (0.45 if index in (0, 1) else -0.45),
truth[index, 1] + vertical_offset,
name, ha=horizontal, va=vertical, fontsize=9,
)
axis_example.invert_yaxis()
axis_example.set_aspect("equal", adjustable="datalim")
axis_example.set_xlabel("图像横坐标 u / px")
axis_example.set_ylabel("图像纵坐标 v / px")
axis_example.set_title("A 一帧真实观测：角点误差是二维向量，不只是一个距离", loc="left",
fontsize=12, fontweight="bold")
axis_example.legend(frameon=False, ncol=1, loc="center")
axis_example.text(0.99, 0.03, "橙色误差箭头放大 4 倍", transform=axis_example.transAxes,
ha="right", color="#6B7280", fontsize=9)
style_axis(axis_example)
axis_cloud = figure.add_subplot(grid[1])
for distance in distances:
rows = [row for row in corner_rows if row["distance_m"] == distance]
values = np.asarray([[row["raw_du_px"], row["raw_dv_px"]] for row in rows])
if len(values) > 2500:
indices = np.linspace(0, len(values) - 1, 2500, dtype=int)
shown = values[indices]
else:
shown = values
color = DISTANCE_COLORS[distance]
axis_cloud.scatter(shown[:, 0], shown[:, 1], s=7, alpha=0.12, color=color,
edgecolors="none")
add_covariance_ellipse(axis_cloud, values, color, "%.0f m：95%% 协方差椭圆" % distance)
all_values = np.asarray([[row["raw_du_px"], row["raw_dv_px"]] for row in corner_rows])
limit = max(3.0, float(np.percentile(np.abs(all_values), 99.0)))
axis_cloud.set_xlim(-limit, limit)
axis_cloud.set_ylim(-limit, limit)
axis_cloud.axhline(0.0, color="#9CA3AF", linewidth=1.0)
axis_cloud.axvline(0.0, color="#9CA3AF", linewidth=1.0)
axis_cloud.set_aspect("equal", adjustable="box")
axis_cloud.set_xlabel("水平残差 Δu = 检测值 − 真值 / px")
axis_cloud.set_ylabel("垂直残差 Δv = 检测值 − 真值 / px")
axis_cloud.set_title("B 原始角点残差云：中心偏移表示偏差，形状表示方向相关性", loc="left",
fontsize=12, fontweight="bold")
axis_cloud.legend(frameon=False, fontsize=8.5, loc="best")
style_axis(axis_cloud)
axis_ecdf = figure.add_subplot(grid[2])
for distance in distances:
values = np.sort(np.asarray([
row["raw_error_px"] for row in corner_rows if row["distance_m"] == distance
]))
cumulative = np.arange(1, len(values) + 1) / float(len(values))
axis_ecdf.plot(values, cumulative, color=DISTANCE_COLORS[distance], linewidth=2.4,
label="%.0f m" % distance)
median = float(np.median(values))
axis_ecdf.scatter([median], [0.5], color=DISTANCE_COLORS[distance], s=35, zorder=4)
max_error = float(np.percentile([
row["raw_error_px"] for row in corner_rows
], 99.5))
axis_ecdf.set_xlim(0.0, max_error)
axis_ecdf.set_ylim(0.0, 1.01)
axis_ecdf.set_xlabel("原始角点径向误差 / px")
axis_ecdf.set_ylabel("累计比例")
axis_ecdf.set_title("C 累积分布：多数误差很小，但尾部不能忽略", loc="left",
fontsize=12, fontweight="bold")
axis_ecdf.legend(frameon=False, ncol=3, loc="lower right")
style_axis(axis_ecdf)
figure.suptitle("原始角点误差的分布规律", fontsize=15, fontweight="bold", y=0.995)
figure.text(
0.5, 0.01,
"每个点都与同一曝光时刻、同一物理装甲板的精确投影比较；未按残差挑选或删除长尾样本。",
ha="center", color="#4B5563", fontsize=9,
)
figure.subplots_adjust(top=0.96, bottom=0.06)
save_figure(figure, os.path.join(image_dir, "pnp-corner-noise-distribution"))
def violin_with_box(axis, groups, positions, color):
parts = axis.violinplot(groups, positions=positions, vert=False, widths=0.72,
showmeans=False, showmedians=False, showextrema=False)
for body in parts["bodies"]:
body.set_facecolor(color)
body.set_edgecolor(color)
body.set_alpha(0.28)
for values, position in zip(groups, positions):
p25, median, p75 = np.percentile(values, (25, 50, 75))
p05, p95 = np.percentile(values, (5, 95))
axis.plot([p05, p95], [position, position], color=color, linewidth=1.5)
axis.plot([p25, p75], [position, position], color=color, linewidth=5.0,
solid_capstyle="butt")
axis.scatter([median], [position], s=28, color=color, edgecolor="white",
linewidth=0.7, zorder=4)
def plot_position_distribution(observation_rows: list[dict], image_dir: str) -> None:
distances = sorted({row["distance_m"] for row in observation_rows})
figure, axes = plt.subplots(3, 1, figsize=(8.2, 10.2), sharey=True)
axis_labels = {
"x": "x：前向，近似深度",
"y": "y：水平横向",
"z": "z：竖直方向",
}
positions = np.arange(len(distances))
for axis_plot, coordinate in zip(axes, "xyz"):
groups = [
np.asarray([
row["error_" + coordinate] * 100.0
for row in observation_rows if row["distance_m"] == distance
])
for distance in distances
]
violin_with_box(axis_plot, groups, positions, AXIS_COLORS[coordinate])
axis_plot.axvline(0.0, color="#111827", linewidth=1.0, linestyle="--")
combined = np.concatenate(groups)
low, high = np.percentile(combined, (0.5, 99.5))
padding = max((high - low) * 0.08, 0.3)
axis_plot.set_xlim(low - padding, high + padding)
axis_plot.set_yticks(positions)
axis_plot.set_yticklabels(["%.0f m" % distance for distance in distances])
axis_plot.set_xlabel("有符号坐标误差 / cm")
axis_plot.set_title(axis_labels[coordinate], loc="left", fontsize=12, fontweight="bold")
style_axis(axis_plot, grid_axis="x")
outside = int(np.sum((combined < low) | (combined > high)))
axis_plot.text(0.99, 0.91, "显示 0.5%%–99.5%%，窗外 %d 点" % outside,
transform=axis_plot.transAxes, ha="right", va="top",
color="#6B7280", fontsize=8.5)
figure.suptitle("PnP 输出的 x / y / z 误差并不同分布", fontsize=15,
fontweight="bold", y=0.995)
figure.text(
0.5, 0.01,
"细线：5%–95%；粗线：25%–75%；白心圆点：中位数。每个子图使用自己的横轴尺度。",
ha="center", color="#4B5563", fontsize=9,
)
figure.tight_layout(rect=(0, 0.04, 1, 0.965))
save_figure(figure, os.path.join(image_dir, "pnp-position-noise-distribution"))
def binned_median(x_values: np.ndarray, y_values: np.ndarray, bins: int = 16):
edges = np.quantile(x_values, np.linspace(0.0, 1.0, bins + 1))
centers = []
medians = []
for index in range(bins):
if index == bins - 1:
mask = (x_values >= edges[index]) & (x_values <= edges[index + 1])
else:
mask = (x_values >= edges[index]) & (x_values < edges[index + 1])
if int(np.sum(mask)) < 5:
continue
centers.append(float(np.median(x_values[mask])))
medians.append(float(np.median(y_values[mask])))
return np.asarray(centers), np.asarray(medians)
def normal_qq(values: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
sorted_values = np.sort(values)
probabilities = (np.arange(len(values)) + 0.5) / float(len(values))
theoretical = normal_distribution.ppf(probabilities)
standardized = (sorted_values - np.mean(sorted_values)) / np.std(sorted_values, ddof=1)
return theoretical, standardized
def plot_propagation_and_time(observation_rows: list[dict], summary: dict, image_dir: str) -> None:
distances = sorted({row["distance_m"] for row in observation_rows})
figure = plt.figure(figsize=(8.2, 12.0))
grid = gridspec.GridSpec(3, 1, hspace=0.42)
axis_scale = figure.add_subplot(grid[0])
for distance in distances:
rows = [row for row in observation_rows if row["distance_m"] == distance]
x_values = np.asarray([row["refined_scale_error"] * 100.0 for row in rows])
y_values = np.asarray([row["relative_los_error"] * 100.0 for row in rows])
if len(rows) > 1200:
indices = np.linspace(0, len(rows) - 1, 1200, dtype=int)
else:
indices = np.arange(len(rows))
axis_scale.scatter(x_values[indices], y_values[indices], s=8, alpha=0.16,
color=DISTANCE_COLORS[distance], edgecolors="none")
centers, medians = binned_median(x_values, y_values)
axis_scale.plot(centers, medians, color=DISTANCE_COLORS[distance], linewidth=2.4,
label="%.0f m 分箱中位数" % distance)
all_x = np.asarray([row["refined_scale_error"] * 100.0 for row in observation_rows])
low, high = np.percentile(all_x, (1, 99))
reference_x = np.linspace(low, high, 100)
axis_scale.plot(reference_x, -reference_x, color="#374151", linestyle="--",
linewidth=1.5, label="小扰动近似：相对深度误差 ≈ -尺度误差")
axis_scale.set_xlim(low, high)
all_y = np.asarray([row["relative_los_error"] * 100.0 for row in observation_rows])
y_low, y_high = np.percentile(all_y, (1, 99))
axis_scale.set_ylim(y_low, y_high)
axis_scale.set_xlabel("PnP 输入四边形的相对尺度误差 / %")
axis_scale.set_ylabel("视线方向相对位置误差 / %")
axis_scale.set_title("A 误差传播：角点改变投影尺度，PnP 随之改变深度", loc="left",
fontsize=12, fontweight="bold")
axis_scale.legend(frameon=False, fontsize=8.3, loc="best")
style_axis(axis_scale)
axis_acf = figure.add_subplot(grid[1])
for distance in distances:
values = summary["by_distance"][str(distance)]["line_of_sight"]["acf_lag_0_to_20"]
axis_acf.plot(np.arange(len(values)), values, color=DISTANCE_COLORS[distance],
linewidth=2.3, marker="o", markersize=3.5, label="%.0f m" % distance)
axis_acf.axhline(0.0, color="#9CA3AF", linewidth=1.0)
axis_acf.set_xlim(0, 20)
axis_acf.set_ylim(-0.15, 1.02)
axis_acf.set_xlabel("滞后帧数")
axis_acf.set_ylabel("自相关系数")
axis_acf.set_title("B 相邻帧误差会延续：观测噪声不是完全独立的白噪声", loc="left",
fontsize=12, fontweight="bold")
axis_acf.legend(frameon=False, ncol=3)
style_axis(axis_acf)
axis_qq = figure.add_subplot(grid[2])
for distance in distances:
values = np.asarray([
row["line_of_sight_error"]
for row in observation_rows if row["distance_m"] == distance
])
theoretical, empirical = normal_qq(values)
indices = np.linspace(0, len(values) - 1, min(450, len(values)), dtype=int)
axis_qq.plot(theoretical[indices], empirical[indices], color=DISTANCE_COLORS[distance],
linewidth=2.0, label="%.0f m" % distance)
reference = np.linspace(-3.3, 3.3, 100)
axis_qq.plot(reference, reference, color="#374151", linestyle="--", linewidth=1.4,
label="理想高斯分布")
axis_qq.set_xlim(-3.3, 3.3)
axis_qq.set_ylim(-6.0, 6.0)
axis_qq.set_xlabel("理论高斯分位数")
axis_qq.set_ylabel("实测标准化分位数")
axis_qq.set_title("C Q–Q 图：尾部偏离直线，固定高斯噪声只是一种近似", loc="left",
fontsize=12, fontweight="bold")
axis_qq.legend(frameon=False, ncol=2, fontsize=8.5)
style_axis(axis_qq)
figure.suptitle("从角点误差到 EKF 观测噪声", fontsize=15, fontweight="bold", y=0.995)
figure.text(
0.5, 0.01,
"尺度—深度关系、时间相关性和长尾共同说明：EKF 的 R 不应脱离距离与观测质量固定设置。",
ha="center", color="#4B5563", fontsize=9,
)
figure.subplots_adjust(top=0.96, bottom=0.06)
save_figure(figure, os.path.join(image_dir, "pnp-noise-ekf-bridge"))
def write_csv(path: str, rows: list[dict]) -> None:
if not rows:
raise ValueError("cannot write an empty CSV")
with open(path, "w", newline="", encoding="utf-8") as handle:
writer = csv.DictWriter(handle, fieldnames=list(rows[0]))
writer.writeheader()
writer.writerows(rows)
def format_number(value: float, digits: int = 3) -> str:
return ("%%.%df" % digits) % float(value)
def write_report(path: str, summary: dict) -> None:
distances = sorted(float(value) for value in summary["by_distance"])
lines = [
"# PnP 角点与坐标噪声探索性分析",
"",
"## Executive Summary",
"",
"本报告使用 2 m、4 m、6 m 三组受控采集，将检测器原始角点、PnP 输入角点、PnP 坐标与同曝光模拟器真值逐帧配对。",
"",
"## Basic Information",
"",
"- 数据格式：Stage-3 observation/truth JSONL；处理结果为 CSV、JSON、PNG 与 SVG。",
"- 匹配观测数：%d。" % summary["overall"]["matched_observation_count"],
"- 独立角点样本数：%d。" % summary["overall"]["corner_sample_count"],
"- 坐标系：tracker/chassis，x 前向、y 横向、z 竖直。",
"- 真值用途：仅用于离线评估，不进入检测器、PnP 或跟踪器。",
"",
"## Data Quality",
"",
"- observation 与 truth 使用 `(producer_epoch, frame_seq, timestamp_ns)` 精确连接。",
"- 物理装甲板按图像中心一对一关联；角点再按检测器屏幕顺序配对。",
"- 关联像素距离中位数为 %s px，95%% 分位为 %s px。" % (
format_number(summary["overall"]["association_distance_px"]["median"]),
format_number(summary["overall"]["association_distance_px"]["p95"]),
),
"- 未删除长尾样本；绘图时只为保持坐标轴可读而裁剪显示范围，统计仍使用全部数据。",
"",
"## Statistical Summary",
"",
"| 距离 | 原始角点误差 p50 / p95 | 修正角点误差 p50 / p95 | |x| p50 / p95 | |y| p50 / p95 | |z| p50 / p95 | LOS lag-1 |",
"| ---: | ---: | ---: | ---: | ---: | ---: | ---: |",
]
for distance in distances:
row = summary["by_distance"][str(distance)]
raw = row["raw_corner"]["radial_px"]
refined = row["refined_corner"]["radial_px"]
coordinates = [row["pnp_position"][axis]["absolute_m"] for axis in "xyz"]
lines.append(
"| %.0f m | %s / %s px | %s / %s px | %s / %s cm | %s / %s cm | %s / %s cm | %s |" % (
distance,
format_number(raw["median"]), format_number(raw["p95"]),
format_number(refined["median"]), format_number(refined["p95"]),
format_number(coordinates[0]["median"] * 100.0, 2),
format_number(coordinates[0]["p95"] * 100.0, 2),
format_number(coordinates[1]["median"] * 100.0, 2),
format_number(coordinates[1]["p95"] * 100.0, 2),
format_number(coordinates[2]["median"] * 100.0, 2),
format_number(coordinates[2]["p95"] * 100.0, 2),
format_number(row["line_of_sight"]["acf_lag_0_to_20"][1]),
)
)
lines.extend([
"",
"## Distribution Findings",
"",
"1. 角点误差是二维、有偏、方向相关且带长尾的分布；只记录一个平均像素误差会丢失这些结构。",
"2. PnP 的三个坐标分量不同方差，前向/近似深度误差明显大于横向与竖直误差，并随距离增长。",
"3. 四边形尺度误差与视线方向位置误差负相关，符合 `ΔZ/Z ≈ -Δs/s` 的一阶近似。",
"4. 连续可见片段存在显著短时自相关；逐帧噪声并非严格独立。",
"5. Q–Q 图尾部偏离高斯直线；EKF 使用高斯模型时需要配合门控、质量条件化或稳健处理。",
"",
"## Recommendations for EKF",
"",
"- 先在统一坐标系估计 `x/y/z` 的观测协方差，不要把三个方向设成同一个方差。",
"- 让观测协方差随距离、投影尺度、角点质量和装甲板姿态变化。",
"- 将长尾交给创新门控或稳健代价处理；不要通过删掉离群点来伪造高斯分布。",
"- 后续评估创新序列时检查均值、方差、白噪声性和分位数覆盖，而不只看轨迹是否平滑。",
"",
"## Reproducibility",
"",
"完整输入哈希、拒绝计数、协方差、偏度、峰度、分位数和自相关序列保存在 `noise-summary.json`。",
"",
])
with open(path, "w", encoding="utf-8") as handle:
handle.write("\n".join(lines))
def main() -> int:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--session", action="append", required=True,
help="DISTANCE=path/to/session_result.json")
parser.add_argument("--output-dir", required=True)
parser.add_argument("--image-dir", required=True)
parser.add_argument("--target-number", type=int, default=3)
parser.add_argument("--font", default=r"C:\Windows\Fonts\simhei.ttf")
args = parser.parse_args()
os.makedirs(args.output_dir, exist_ok=True)
os.makedirs(args.image_dir, exist_ok=True)
configure_chinese_font(args.font)
sessions = []
for item in args.session:
distance_text, separator, path = item.partition("=")
if not separator:
parser.error("invalid --session: %s" % item)
sessions.append((float(distance_text), os.path.abspath(path)))
sessions.sort(key=lambda item: item[0])
observation_rows = []
corner_rows = []
capture = {}
for distance, session_path in sessions:
observations, corners, session_capture = load_session(
distance, session_path, args.target_number
)
if not observations or not corners:
raise ValueError("session at %.1f m produced no matched data" % distance)
observation_rows.extend(observations)
corner_rows.extend(corners)
capture[str(distance)] = session_capture
summary = summarize(observation_rows, corner_rows, capture)
write_csv(os.path.join(args.output_dir, "observation-noise-samples.csv"), observation_rows)
write_csv(os.path.join(args.output_dir, "corner-noise-samples.csv"), corner_rows)
with open(os.path.join(args.output_dir, "noise-summary.json"), "w", encoding="utf-8") as handle:
json.dump(summary, handle, ensure_ascii=False, indent=2, sort_keys=True, allow_nan=False)
handle.write("\n")
write_report(os.path.join(args.output_dir, "eda-report.md"), summary)
plot_corner_distribution(corner_rows, args.image_dir)
plot_position_distribution(observation_rows, args.image_dir)
plot_propagation_and_time(observation_rows, summary, args.image_dir)
print(json.dumps(summary, ensure_ascii=False, indent=2, sort_keys=True, allow_nan=False))
return 0
if __name__ == "__main__":
raise SystemExit(main())
manifests/distance-2m.json
{
"schema_version": "stage3-manifest-v1",
"dataset_id": "tutorial-pnp-coordinate-error-v1",
"session_id": "tutorial-pnp-coordinate-error-v1-d2",
"target_number": 3,
"camera_profile": "wide_6mm",
"dual_focal": false,
"distance_m": 2.0,
"initial_yaw_rad": 0.0,
"mode": "spin",
"direction_deg": 0.0,
"linear_speed_mps": 0.0,
"linear_span_m": 0.0,
"spin_rad_s": 0.5235987755982988
}
manifests/distance-4m.json
{
"schema_version": "stage3-manifest-v1",
"dataset_id": "tutorial-pnp-coordinate-error-v1",
"session_id": "tutorial-pnp-coordinate-error-v1-d4",
"target_number": 3,
"camera_profile": "wide_6mm",
"dual_focal": false,
"distance_m": 4.0,
"initial_yaw_rad": 0.0,
"mode": "spin",
"direction_deg": 0.0,
"linear_speed_mps": 0.0,
"linear_span_m": 0.0,
"spin_rad_s": 0.5235987755982988
}
manifests/distance-6m.json
{
"schema_version": "stage3-manifest-v1",
"dataset_id": "tutorial-pnp-coordinate-error-v1",
"session_id": "tutorial-pnp-coordinate-error-v1-d6",
"target_number": 3,
"camera_profile": "wide_6mm",
"dual_focal": false,
"distance_m": 6.0,
"initial_yaw_rad": 0.0,
"mode": "spin",
"direction_deg": 0.0,
"linear_speed_mps": 0.0,
"linear_span_m": 0.0,
"spin_rad_s": 0.5235987755982988
}