圆心标定方案

src/g20_thumb_apriltag_calibration/README.md

@@ -3,17 +3,36 @@
 该包启动 RealSense、彩色图像校正、`apriltag_ros`、Linker Hand SDK 和标定状态机，
 只扫描 G20 左手命令下标 `0`、`15`。默认使用单终点连续模式：每个方向只发送一次
 终点命令，速度保持在固件能稳定响应的 `15`。SDK 以独立时间戳反馈实际 20 维位置，
-程序把每帧 AprilTag 角度与同一时刻的实际电机位置插值配对并按整数位置分箱。
+程序把每帧 AprilTag 三维中心与同一时刻的实际电机位置插值配对并按整数位置分箱。
 完成 `255→0→255` 后分别拟合正反方向并检查回差，最终运行时 JSON 将两条曲线逐点
 平均，只为每个关节保存一个 256 项 `angle_rad`。最后用 5 个随机静态命令复测精度。
 
-当前正面单机位使用 `image_plane_2d` 模式：从四个有序角点计算 Tag 在校正图像平面内的
-方向，再求 T0→T3、T3→T4、T4→T5 的相对转角。该模式只用于正面屈伸角，不估计侧摆、
-横摆或出平面旋转；AprilTag 的 PnP/TF 仍保留作诊断和数据归档。
+当前默认使用 `trajectory_center_3d`。节点由四个亚像素角点和 `CameraInfo.P`
+计算每张 Tag 的三维中心，但不把小尺寸平面 Tag 的 PnP 朝向直接当作关节角：
+
+- 根部扫描先减去掌心 T0 的位置，再用 T3/T4/T5 三条圆轨迹共同拟合 CMC 旋转轴；
+  每帧三个角度取中位数。
+- 尖部扫描用 T4 相对 T3 的圆轨迹直接拟合 MCP。G20 只有电机 15 这一个尖部输入，
+  URDF 将被动 IP 定义为 `thumb_ip = 1.02 × thumb_mcp`，因此运行时 IP 曲线严格按
+  这个机械耦合生成。这样不会把不同相机角度下 T5 的平面 PnP 深度偏差误认为 IP
+  真实运动。
+- 程序仍会按 MCP 角将 T5 反向旋转并拟合剩余小圆，但该结果只用于
+  `trajectory_center_quality.tip` 中的观测一致性诊断，不参与最终 IP 数组。
+- 每条曲线都减去命令 255 的测量角，所以最终文件严格满足
+  `angle_rad[255] == 0.0`；`angle_rad[0]` 是该关节相对零位的最大角度。
+
+这种方法对固定的相机摆放角度、Tag 在同一刚性连杆上的固定位置和贴纸朝向更不敏感。
+但相机或贴纸在一次扫描过程中移动、Tag 翘起、角点严重抖动仍会破坏圆轨迹。程序会
+检查平面残差、圆残差、轨迹半径、实际弧长和根部三个轨迹点的角度一致性。
+
+PnP 双分支跟踪和整段刚体复核仍保留，用于选出稳定的三维中心及辅助质量检查，不再
+直接生成运行时角度。根部扫描用固定的 T3–T4、T4–T5 中心间距共同选择分支；
+尖部扫描用固定的 T0–T3 中心间距约束非目标部分。中心间距漂移超过阈值仍会暂停，
+避免错误中心进入圆拟合，但 Tag 的 PnP 朝向抖动不会触发该门限。
 
 ## 1. 标记和安全检查
 
-- `T0` 固定在掌壳，`T3` 固定在拇指根部运动连杆，`T4` 固定在 MCP 后的连杆，
+- `T0` 必须保留并固定在掌壳，作为整体平移参考；`T3` 固定在拇指根部运动连杆，`T4` 固定在 MCP 后的连杆，
   `T5` 固定在最末节。四张 Tag 必须与所在刚性件完全固定，不能跨关节或贴在软胶上。
 - 当前实物使用 `tag36h11` 的 ID `0/1/2/3`，依次对应 T0/T3/T4/T5。如果实物 ID 改变，同时修改
   `config/front_tags.yaml` 里检测节点和标定节点的两组数组。
@@ -68,6 +87,7 @@ ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
   can_interface:=can0 \
   calibration_speed:=15 \
   continuous_motion_mode:=endpoint \
+  angle_estimation_mode:=trajectory_center_3d \
   apriltag_decimate:=1.5 \
   use_roi:=false
 ```
@@ -80,6 +100,9 @@ ros2 run image_view image_view --ros-args \
   --remap image:=/camera/camera/color/image_rect
 ```
 
+图像检测链路使用 `sensor_data`（BEST_EFFORT）QoS，只保留最新帧，避免完整分辨率
+下可靠队列积压反压相机；这不会裁剪图像，也不会降低相机分辨率。
+
 若以后需要以帧率优先，可传入 `use_roi:=true`；默认 ROI 是原图中的
 `x=128, y=192, width=1024, height=528`，也可用 `roi_x`、`roi_y`、
 `roi_width`、`roi_height` 覆盖。
@@ -109,13 +132,24 @@ ros2 service call /g20_thumb_calibration/resume std_srvs/srv/Trigger {}
 ros2 service call /g20_thumb_calibration/abort std_srvs/srv/Trigger {}
 ```
 
-预检要求四 Tag 有效帧率至少 95%，且检测消息频率至少 15 Hz。连续扫描要求
+预检要求四 Tag 有效帧率至少 95%，且检测消息频率至少 15 Hz。PnP 有效率也必须
+至少 95%，每个候选解的重投影 RMS 不超过 1.5 px。中心轨迹模式以三组相对中心
+的静止 RMS 不超过 2 mm、5 mm 范围内位置内点不少于 90% 为硬判据；PnP 朝向抖动
+只作为诊断，不会阻止静态捕获。
+状态中的
+`pnp_rejections` 会指出当前是哪张 Tag 因丢失、重投影/倾角超限或姿态跳变而被拒绝，
+`pnp_reprojection_error_px` 显示四张 Tag 最近一次有效解的误差。连续扫描要求
 图像与状态的时间差不超过 150 ms、全行程至少得到 40 个有效帧、
 至少覆盖 32 个整数位置且相邻实测位置间隔不超过 16。Tag 或同步状态持续丢失 3 秒、
 90 秒内未到达终点，或覆盖不足时，节点保持当前命令并进入 `PAUSED`。恢复时会先回到
 该方向的起点，再完整重扫这个方向，避免把半程数据混入结果。`abort` 也只停止队列，
 不会主动移动机械手。正常扫描和随机复测最后一项均为命令 255。
 
+PnP 跟踪在整个会话中对四张 Tag 都优先保持同一个 IPPE 平面分支；最多 5 秒的短暂检测
+间隔不会重新初始化分支。随机复测只有在同步电机反馈与目标相差不超过 2、且稳定
+窗口与捕获窗口内三个相对中心的最大偏差都不超过 3 mm 时才会写入，否则继续等待并最终暂停，
+不会再生成明知不可靠但字段完整的结果。
+
 单终点连续模式共有 4 个端到端命令：根部和尖部各一个往返。每个方向运动前会先用
 实际电机反馈确认已经到达起点，再做一次短暂静态确认；随机验证的“接近位置”只等待
 电机反馈到位，不再重复采图。若实际 AprilTag 检测仍低于 15 Hz，先优化检测链路，
@@ -124,10 +158,30 @@ ros2 service call /g20_thumb_calibration/abort std_srvs/srv/Trigger {}
 
 连续扫描中的主要状态字段：
 
+- `state_zh`/`reason_zh`/`action_zh`：当前阶段、失败原因和下一步操作的中文说明；
+  原有 `state`/`reason` 英文机器码继续保留。
+- `tag_quality`：逐张显示 T0/T3/T4/T5 的边长、hamming、识别置信度、重投影误差、
+  是否有效和具体中文问题，不再需要手工解析 `/apriltag/detections`。
+- `/g20_thumb_calibration/status_text`：适合终端直接查看的多行中文状态。使用
+  `ros2 topic echo --once /g20_thumb_calibration/status_text --field data`
+  即可看到原因、建议及四张标签的质量。
 - `scan_progress`：4 个方向的完成比例，依次约为 0、0.25、0.5、0.75、1.0。
 - `sweep_valid_frames_seen`：当前连续方向已收到的同步有效帧数。
 - `sweep_state_span_u8`：当前方向实际覆盖的电机范围，接近 255 才算完整。
 - `active_phase`/`active_direction`：当前是根部或尖部、下降或上升方向。
+- `pnp_branch_corrections`：四张 Tag 联合跟踪为维持相邻关节姿态连续，而没有选择
+  单张 Tag 最小重投影分支的累计次数。
+- `pnp_trajectory_quality`：最近一个完整方向的整段分支修正帧数，以及相对整段稳健
+  参考的旋转、相对平移和中心间距漂移。中心轨迹模式只按欧氏中心间距判断：
+  P95 超过 3 mm 或单帧最大值超过 6 mm 时暂停；旋转及随 Tag 坐标轴表达的相对平移
+  只保留为诊断。
+- `trajectory_center_quality`：四个方向完成并拟合后，显示三维平面/圆残差、拟合半径、
+  实际弧长、T0/T3 锚点漂移和根部三个轨迹点的角度一致性。其中
+  `tip.ip_observed_vs_constrained_*` 显示T5残余小圆与URDF被动耦合之间的差异；
+  它用于发现T5识别误差、标签松动或机构异常，但不会改变最终IP曲线。
+
+根部扫描中 T3/T4/T5 作为完整刚性组共同选择 IPPE 分支，不再把 T3 固定为在线解；
+尖部扫描仍固定 T3，只用静止的 T0/T3 约束修正非目标根部姿态。
 
 ## 4. 中断恢复和输出
 
@@ -151,7 +205,7 @@ ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
 
 目录内文件：
 
-- `raw_samples.jsonl`：连续帧按实际整数电机位置分箱后的鲁棒统计及复测点；每完成一个
+- `raw_samples.jsonl`：连续帧按实际整数电机位置分箱后的 Tag 三维中心、姿态辅助统计及复测点；每完成一个
   扫描方向后落盘。
 - `checkpoint.json`：当前状态和进度。
 - `session_manifest.json`：Tag、相机内参、SDK、代码哈希和会话信息。
@@ -168,9 +222,24 @@ ros2 launch g20_thumb_apriltag_calibration front_thumb_calibration.launch.py \
 }
 ```
 
+`thumb_ip.angle_rad` 由 `thumb_mcp.angle_rad` 乘
+`ip_coupling.multiplier`（默认 `1.02`）得到，二者在命令255处都严格为零。
+
 `thumb_ip` 另外包含 `"passive": true`。正反方向原始曲线不进入最终 JSON，但仍保留
 在 `raw_samples.jsonl` 中，并用于最大回差和质量判定。
 
+零位和最大角度可直接读取：
+
+```python
+import json
+from pathlib import Path
+
+data = json.loads(Path("g20_left_G20_LEFT_001_thumb_angle.json").read_text())
+for name, joint in data["joints"].items():
+    print(name, "zero(rad)=", joint["angle_rad"][255],
+          "max(rad)=", joint["angle_rad"][0])
+```
+
 如果相机或 SDK 已由外部进程启动，可传
 `start_camera:=false` 或 `start_sdk:=false`。用 `camera_serial_number:=<序列号>`
 可绑定指定 RealSense。
@@ -181,15 +250,18 @@ D405 的彩色流来自 `depth_module`，启动文件会同时设置
 它们都不参与角度计算。需要完整诊断留档时可增加
 `enable_depth:=true record_bag:=true`。
 
-默认对完整 1280×720 原图进行畸变校正和 AprilTag 检测。校正和 AprilTag 组件运行
+默认对完整 1280×720 原图进行畸变校正和 AprilTag 检测。三维中心 PnP 必须使用
+`image_rect` 的角点及同一条处理链对应的 `CameraInfo`，启动文件已自动保证二者配对。
+校正和 AprilTag 组件运行
 在同一个多线程容器内并启用进程内传输，避免在处理链路中重复序列化、复制大图像。
 可选 ROI 模式会额外在同一容器内加入裁剪组件并同步修正 `CameraInfo`。标定节点默认
 不订阅整幅图像，只订阅检测结果和 TF。
 若启用调试图，预览会缩放到 50%、限速 10 Hz 并使用最新帧优先的传输方式，
 不影响 AprilTag 的 ROI 输入。
 
-静态预检会把偏离鲁棒姿态超过 5° 的平面 PnP 瞬时翻解视为异常帧，但要求姿态内点率
-至少 95%；内点自身仍必须满足 0.5° RMS，避免用异常过滤掩盖真实抖动。
+静态预检先在单 Tag 层拒绝高重投影误差，再检查三组相对中心的位置内点率和毫米级 RMS。
+当前 30～38 px 的 10 mm Tag 属于试标定尺寸，如果中心位置 RMS 持续不合格，应优先增加照明、缩短
+相机距离或提高 Tag 有效像素，而不是放宽最终随机复测精度。
 
 启用 rosbag 后保存裁剪后的原始图像和配套 `CameraInfo`，避免新增一个全分辨率图像
 订阅者；同时使用 MCAP `zstd_fast` 压缩并按 10 GiB 分卷。快速标定通常不需要录制；

src/g20_thumb_apriltag_calibration/config/calibration.yaml

@@ -7,7 +7,12 @@ g20_thumb_calibration:
     image_topic: /camera/camera/color/image_rect
     detections_topic: /apriltag/detections
     tf_topic: /tf
-    angle_estimation_mode: image_plane_2d
+    # Use PnP translations as 3-D Tag centres and fit the directly observable
+    # root/MCP circles. The passive IP output follows the G20 URDF mimic
+    # relation below; its small residual T5 circle is diagnostic only. PnP
+    # orientations remain auxiliary quality checks. Command 255 is zero.
+    angle_estimation_mode: trajectory_center_3d
+    passive_ip_multiplier: 1.02
     publish_debug_image: false
     debug_max_rate_hz: 10.0
     debug_scale: 0.5
@@ -42,15 +47,77 @@ g20_thumb_calibration:
     # Trial threshold for the current 10 mm tags (observed at 32-38 px).
     # Final acceptance is still guarded by static RMS and random validation.
     minimum_edge_pixels: 30.0
-    maximum_static_std_deg: 0.5
+    # Current 30 px tags measure about 0.50-0.53 deg RMS while stationary.
+    # Keep a small practical margin here; final random validation stays at
+    # MAE <= 2 deg and P95 <= 3 deg.
+    # Match the preflight noise gate to the 3 deg robust capture gate below.
+    # The final calibration is still accepted only by the independent
+    # validation MAE/P95 limits, not by this readiness check.
+    maximum_static_std_deg: 3.0
     pose_outlier_threshold_deg: 5.0
-    minimum_pose_inlier_rate: 0.95
+    minimum_pose_inlier_rate: 0.90
+    pnp_minimum_valid_rate: 0.95
+    # 30-38 px tags are usable, but only if IPPE gives a tight image fit and
+    # a pose continuous with the preceding frame.
+    pnp_maximum_reprojection_error_px: 1.5
+    # All four tags keep a temporally continuous IPPE solution throughout the
+    # complete session. With 30 px planar tags, tiny reprojection differences
+    # do not reliably identify the physical branch and previously caused
+    # stationary T0 to flip by about 25 deg between scan and validation.
+    pnp_reprojection_tie_px: 1.5
+    pnp_maximum_pose_jump_deg: 35.0
+    pnp_maximum_translation_jump_m: 0.04
+    pnp_maximum_tag_tilt_deg: 75.0
+    # Preserve the branch through short detector gaps. A continuous sweep
+    # already pauses after 3 s without valid synchronised observations.
+    pnp_tracker_reset_seconds: 5.0
+    # Select all four IPPE branches as one kinematic chain. This prevents T4
+    # and T5 from independently changing mirror branches at the turnaround or
+    # during validation while still allowing real joint motion frame-to-frame.
+    pnp_group_relative_rotation_scale_deg: 5.0
+    pnp_group_relative_translation_scale_m: 0.01
+    # Reprojection remains a tie-breaker; temporal joint-chain continuity is
+    # deliberately dominant for the current 30-38 px planar tags.
+    pnp_group_reprojection_weight: 0.05
+    # Whole-sweep branch review. During a root sweep T3/T4/T5 should retain
+    # rigid relative poses; during a tip sweep T0/T3 should remain fixed.
+    pnp_trajectory_reprojection_scale_px: 0.1
+    pnp_rigid_rotation_scale_deg: 5.0
+    pnp_rigid_translation_scale_m: 0.01
+    # Judge the complete rigid trajectory against a robust sweep reference.
+    # Reject persistent drift at P95; keep a looser hard maximum so one noisy
+    # 30 px endpoint frame does not discard an otherwise sound sweep.
+    pnp_rigid_p95_accepted_drift_deg: 8.0
+    pnp_rigid_maximum_accepted_drift_deg: 15.0
+    # Centre-trajectory mode judges branch consistency by the Euclidean
+    # distance between rigid Tag centres.  This is deliberately independent
+    # of the noisy planar-Tag orientation returned by PnP.
+    pnp_rigid_p95_accepted_distance_drift_m: 0.003
+    pnp_rigid_maximum_accepted_distance_drift_m: 0.006
+
+    # Three-dimensional centre-trajectory geometry gates. T0 stays on the
+    # palm as the translation anchor; T3/T4/T5 are the moving thumb points.
+    trajectory_maximum_plane_rms_m: 0.004
+    trajectory_maximum_radial_rms_m: 0.004
+    trajectory_minimum_radius_m: 0.005
+    trajectory_minimum_arc_deg: 15.0
+    trajectory_maximum_root_role_disagreement_deg: 5.0
+    trajectory_maximum_anchor_drift_m: 0.005
+    trajectory_static_translation_outlier_m: 0.005
+    trajectory_maximum_static_translation_rms_m: 0.002
 
     # Static captures are now used only for sweep preparation and validation.
     stable_frames: 5
     capture_frames: 8
     minimum_settle_seconds: 0.4
-    maximum_stable_spread_deg: 0.3
+    # This only confirms that the hand has stopped before an 8-frame robust
+    # median capture. The passive T4->T5 pair currently has about 2.3 deg
+    # peak spread over five 30 px PnP frames, while its two IPPE branches are
+    # separated by about 5.5 deg. A 3 deg gate accepts measurement jitter but
+    # still rejects a branch change. Final MAE/P95 limits remain unchanged.
+    maximum_stable_spread_deg: 3.0
+    # In centre mode the stationary capture gate is expressed in metres.
+    maximum_stable_translation_spread_m: 0.003
     settle_timeout_seconds: 10.0
     capture_timeout_seconds: 10.0
 

src/g20_thumb_apriltag_calibration/config/front_tags.yaml

@@ -1,6 +1,10 @@
 /apriltag/apriltag:
   ros__parameters:
     image_transport: raw
+    # Live calibration needs the newest frame, not lossless delivery of stale
+    # frames. BEST_EFFORT prevents a slow full-resolution detection callback
+    # from back-pressuring image_proc's reliable image publisher.
+    qos_profile: sensor_data
     family: 36h11
     size: 0.01
     profile: false

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/acquisition.py

@@ -4,12 +4,20 @@ from __future__ import annotations
 
 from bisect import bisect_left
 from collections import deque
-from dataclasses import dataclass
+from dataclasses import dataclass, field
 from typing import Any, Mapping, Sequence
 
 import numpy as np
 
 from .core import PAIR_NAMES, robust_rotation_summary
+from .pnp import SquareTagPose
+
+
+TAG_PAIR_ROLES: dict[str, tuple[str, str]] = {
+    "t0_t3": ("t0", "t3"),
+    "t3_t4": ("t3", "t4"),
+    "t4_t5": ("t4", "t5"),
+}
 
 
 @dataclass(frozen=True)
@@ -17,6 +25,7 @@ class TagQuality:
     hamming: int
     decision_margin: float
     edge_pixels: float
+    reprojection_error_px: float | None = None
 
 
 def tag_quality_is_valid(
@@ -25,12 +34,21 @@ def tag_quality_is_valid(
     maximum_hamming: int,
     minimum_decision_margin: float,
     minimum_edge_pixels: float,
+    maximum_reprojection_error_px: float | None = None,
 ) -> bool:
-    return (
+    detection_valid = (
         quality.hamming <= maximum_hamming
         and quality.decision_margin >= minimum_decision_margin
         and quality.edge_pixels >= minimum_edge_pixels
     )
+    if not detection_valid:
+        return False
+    if maximum_reprojection_error_px is None:
+        return True
+    return (
+        quality.reprojection_error_px is not None
+        and quality.reprojection_error_px <= maximum_reprojection_error_px
+    )
 
 
 @dataclass(frozen=True)
@@ -42,6 +60,15 @@ class Observation:
     state_u8: tuple[float, ...] = ()
     state_stamp_ns: int | None = None
     state_sync_error_ns: int | None = None
+    tag_quaternion_xyzw: Mapping[
+        str, tuple[float, float, float, float]
+    ] = field(default_factory=dict)
+    tag_translation_xyz_m: Mapping[
+        str, tuple[float, float, float]
+    ] = field(default_factory=dict)
+    tag_pose_candidates: Mapping[
+        str, tuple[SquareTagPose, ...]
+    ] = field(default_factory=dict)
 
 
 @dataclass(frozen=True)
@@ -273,6 +300,8 @@ class PointCollector:
         capture_frames: int = 30,
         minimum_settle_seconds: float = 0.4,
         maximum_stable_spread_rad: float = np.deg2rad(0.3),
+        stability_mode: str = "rotation",
+        maximum_stable_translation_spread_m: float = 0.003,
         settle_timeout_seconds: float = 5.0,
         capture_timeout_seconds: float = 5.0,
     ) -> None:
@@ -282,6 +311,18 @@ class PointCollector:
         self.capture_frames = int(capture_frames)
         self.minimum_settle_seconds = float(minimum_settle_seconds)
         self.maximum_stable_spread_rad = float(maximum_stable_spread_rad)
+        self.stability_mode = str(stability_mode)
+        self.maximum_stable_translation_spread_m = float(
+            maximum_stable_translation_spread_m
+        )
+        if self.stability_mode not in {"rotation", "translation"}:
+            raise ValueError(
+                "stability_mode must be rotation or translation"
+            )
+        if self.maximum_stable_translation_spread_m <= 0.0:
+            raise ValueError(
+                "maximum_stable_translation_spread_m must be positive"
+            )
         self.settle_timeout_seconds = float(settle_timeout_seconds)
         self.capture_timeout_seconds = float(capture_timeout_seconds)
         self._stable: deque[Observation] = deque(maxlen=self.stable_frames)
@@ -291,8 +332,40 @@ class PointCollector:
         self.capture_started_at: float | None = None
         self.state = "idle"
         self.reason = ""
+        self.stable_spread_rad: dict[str, float] = {}
+        self.stable_spread_m: dict[str, float] = {}
+        self.required_state_index: int | None = None
+        self.required_state_u8: float | None = None
+        self.maximum_state_error_u8: float | None = None
 
-    def start(self, now: float) -> None:
+    def start(
+        self,
+        now: float,
+        *,
+        required_state_index: int | None = None,
+        required_state_u8: float | None = None,
+        maximum_state_error_u8: float | None = None,
+    ) -> None:
+        state_constraints = (
+            required_state_index,
+            required_state_u8,
+            maximum_state_error_u8,
+        )
+        if any(value is not None for value in state_constraints) and not all(
+            value is not None for value in state_constraints
+        ):
+            raise ValueError(
+                "point state constraint parameters must be provided together"
+            )
+        if required_state_index is not None:
+            if not 0 <= int(required_state_index) < 20:
+                raise ValueError("required_state_index must be in [0, 19]")
+            if not np.isfinite(float(required_state_u8)):
+                raise ValueError("required_state_u8 must be finite")
+            if float(maximum_state_error_u8) < 0.0:
+                raise ValueError(
+                    "maximum_state_error_u8 must be non-negative"
+                )
         self._stable.clear()
         self._captured.clear()
         self._consecutive_invalid_frames = 0
@@ -300,6 +373,21 @@ class PointCollector:
         self.capture_started_at = None
         self.state = "settling"
         self.reason = ""
+        self.stable_spread_rad = {}
+        self.stable_spread_m = {}
+        self.required_state_index = (
+            None
+            if required_state_index is None
+            else int(required_state_index)
+        )
+        self.required_state_u8 = (
+            None if required_state_u8 is None else float(required_state_u8)
+        )
+        self.maximum_state_error_u8 = (
+            None
+            if maximum_state_error_u8 is None
+            else float(maximum_state_error_u8)
+        )
 
     @property
     def active(self) -> bool:
@@ -316,17 +404,80 @@ class PointCollector:
     def _window_is_stable(self) -> bool:
         if len(self._stable) < self.stable_frames:
             return False
+        if self.stability_mode == "translation":
+            spreads: dict[str, float] = {}
+            for pair, (parent, child) in TAG_PAIR_ROLES.items():
+                if any(
+                    parent not in observation.tag_translation_xyz_m
+                    or child not in observation.tag_translation_xyz_m
+                    for observation in self._stable
+                ):
+                    self.reason = f"{pair}_translation_missing"
+                    return False
+                vectors = np.asarray(
+                    [
+                        np.asarray(
+                            observation.tag_translation_xyz_m[child],
+                            dtype=float,
+                        )
+                        - np.asarray(
+                            observation.tag_translation_xyz_m[parent],
+                            dtype=float,
+                        )
+                        for observation in self._stable
+                    ],
+                    dtype=float,
+                )
+                reference = np.median(vectors, axis=0)
+                spreads[pair] = float(
+                    np.max(np.linalg.norm(vectors - reference, axis=1))
+                )
+            self.stable_spread_m = spreads
+            for pair, spread in spreads.items():
+                if spread > self.maximum_stable_translation_spread_m:
+                    self.reason = f"{pair}_not_stable"
+                    return False
+            return True
+        spreads: dict[str, float] = {}
         for pair in PAIR_NAMES:
             quaternions = [
                 observation.relative_quaternion_xyzw[pair]
                 for observation in self._stable
             ]
             _, spread = robust_rotation_summary(quaternions)
+            spreads[pair] = float(spread)
+        self.stable_spread_rad = spreads
+        for pair, spread in spreads.items():
             if spread > self.maximum_stable_spread_rad:
                 self.reason = f"{pair}_not_stable"
                 return False
         return True
 
+    def _state_is_acceptable(self, observation: Observation) -> bool:
+        if self.required_state_index is None:
+            return True
+        if (
+            len(observation.state_u8) != 20
+            or observation.state_sync_error_ns is None
+        ):
+            return False
+        value = float(observation.state_u8[self.required_state_index])
+        return bool(
+            np.isfinite(value)
+            and abs(value - float(self.required_state_u8))
+            <= float(self.maximum_state_error_u8)
+        )
+
+    def _return_to_settling(self, reason: str) -> None:
+        self._stable.clear()
+        self._captured.clear()
+        self._consecutive_invalid_frames = 0
+        self.capture_started_at = None
+        self.state = "settling"
+        self.reason = str(reason)
+        self.stable_spread_rad = {}
+        self.stable_spread_m = {}
+
     def add(
         self, observation: Observation, now: float
     ) -> dict[str, Any] | None:
@@ -334,6 +485,9 @@ class PointCollector:
             return None
         self._consecutive_invalid_frames = 0
         now = float(now)
+        if not self._state_is_acceptable(observation):
+            self._return_to_settling("motor_position_out_of_tolerance")
+            return None
         if self.state == "settling":
             self._stable.append(observation)
             elapsed = now - float(self.started_at)
@@ -347,6 +501,35 @@ class PointCollector:
         self._captured.append(observation)
         if len(self._captured) < self.capture_frames:
             return None
+        # Validate continuity across the boundary as well as inside the
+        # capture block. A planar branch can switch immediately after the
+        # stable window and then look perfectly stable for every capture
+        # frame; checking only the captured frames would accept that jump.
+        stability_aggregate = aggregate_observations(
+            [*self._stable, *self._captured]
+        )
+        if self.stability_mode == "translation":
+            unstable_pairs = [
+                pair
+                for pair, spread in stability_aggregate[
+                    "maximum_translation_spread_m"
+                ].items()
+                if float(spread)
+                > self.maximum_stable_translation_spread_m
+            ]
+        else:
+            unstable_pairs = [
+                pair
+                for pair, spread in stability_aggregate[
+                    "maximum_spread_rad"
+                ].items()
+                if float(spread) > self.maximum_stable_spread_rad
+            ]
+        if unstable_pairs:
+            self._return_to_settling(
+                f"{unstable_pairs[0]}_capture_not_stable"
+            )
+            return None
         self.state = "complete"
         return aggregate_observations(self._captured)
 
@@ -365,6 +548,9 @@ class PointCollector:
 
     def mark_invalid_frame(self) -> None:
         """Skip one invalid frame while retaining the recent valid window."""
+        if self.state == "capturing":
+            self._return_to_settling("invalid_tag_frame")
+            return
         if self.state == "settling":
             self._consecutive_invalid_frames += 1
             if self._consecutive_invalid_frames >= 3:
@@ -402,6 +588,15 @@ def aggregate_observations(
             "minimum_edge_pixels": float(min(value.edge_pixels for value in values)),
             "maximum_hamming": int(max(value.hamming for value in values)),
         }
+        reprojection_errors = [
+            float(value.reprojection_error_px)
+            for value in values
+            if value.reprojection_error_px is not None
+        ]
+        if reprojection_errors:
+            quality[tag_name]["maximum_reprojection_error_px"] = float(
+                max(reprojection_errors)
+            )
 
     states = [
         observation.state_u8
@@ -419,6 +614,54 @@ def aggregate_observations(
         for observation in observations
         if observation.state_sync_error_ns is not None
     ]
+    tag_translations: dict[str, list[float]] = {}
+    translation_spread: dict[str, float] = {}
+    translation_roles = sorted(
+        set.intersection(
+            *(
+                set(observation.tag_translation_xyz_m)
+                for observation in observations
+            )
+        )
+        if observations
+        else set()
+    )
+    for role in translation_roles:
+        values = np.asarray(
+            [
+                observation.tag_translation_xyz_m[role]
+                for observation in observations
+            ],
+            dtype=float,
+        )
+        if values.shape == (len(observations), 3) and np.all(
+            np.isfinite(values)
+        ):
+            tag_translations[role] = [
+                float(value)
+                for value in np.median(values, axis=0)
+            ]
+    for pair, (parent, child) in TAG_PAIR_ROLES.items():
+        if parent not in translation_roles or child not in translation_roles:
+            continue
+        vectors = np.asarray(
+            [
+                np.asarray(
+                    observation.tag_translation_xyz_m[child],
+                    dtype=float,
+                )
+                - np.asarray(
+                    observation.tag_translation_xyz_m[parent],
+                    dtype=float,
+                )
+                for observation in observations
+            ],
+            dtype=float,
+        )
+        reference = np.median(vectors, axis=0)
+        translation_spread[pair] = float(
+            np.max(np.linalg.norm(vectors - reference, axis=1))
+        )
 
     return {
         "stamp_start_ns": int(observations[0].stamp_ns),
@@ -428,6 +671,8 @@ def aggregate_observations(
         "maximum_spread_rad": spread,
         "tag_quality": quality,
         "state_u8_median": state_median,
+        "tag_translation_xyz_m": tag_translations,
+        "maximum_translation_spread_m": translation_spread,
         "maximum_state_sync_error_ms": (
             None
             if not sync_errors

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/core.py

@@ -6,7 +6,7 @@ output schema can be tested without a camera or a connected hand.
 
 from __future__ import annotations
 
-from dataclasses import dataclass
+from dataclasses import dataclass, field
 import math
 from typing import Any, Iterable, Mapping, Sequence
 
@@ -323,6 +323,9 @@ class FitResult:
     ip_coupling: dict[str, float]
     max_monotonic_correction_rad: float
     max_hysteresis_rad: float
+    measurement_mode: str = "rotation"
+    trajectory_models: dict[str, Any] = field(default_factory=dict)
+    trajectory_quality: dict[str, Any] = field(default_factory=dict)
 
     def measure_from_reference(
         self,

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/diagnostics.py

@@ -0,0 +1,287 @@
+"""Human-readable diagnostics for the calibration status topic."""
+
+from __future__ import annotations
+
+from typing import Any, Mapping
+
+from .acquisition import TagQuality
+
+
+ROLE_NAMES_ZH = {
+    "t0": "掌心T0",
+    "t3": "拇指根部T3",
+    "t4": "拇指中节T4",
+    "t5": "拇指末节T5",
+}
+
+STATE_NAMES_ZH = {
+    "PREFLIGHT": "设备和标签预检",
+    "WAIT_ROOT_CONFIRM": "等待开始根部标定",
+    "SCAN_ROOT": "正在标定拇指根部",
+    "WAIT_TIP_CONFIRM": "等待开始尖部标定",
+    "SCAN_TIP": "正在标定拇指尖部",
+    "VALIDATING": "正在随机复测",
+    "PAUSED": "标定已暂停",
+    "ABORTED": "标定已终止",
+    "COMPLETE": "标定已完成",
+}
+
+_REJECTION_ZH = {
+    "camera_info_not_ready": "相机内参尚未就绪",
+    "tag_not_detected": "当前画面未检测到标签",
+    "tag_quality_invalid": "标签图像质量未达到门限",
+    "no_pose_within_reprojection_or_tilt_limit": (
+        "PnP重投影误差过大或标签倾斜角过大"
+    ),
+    "pose_jump": "PnP位置或姿态发生突跳",
+    "pnp_solve_failed": "PnP三维位置求解失败",
+    "group_missing_pose_candidates": "标签组缺少可用的PnP候选解",
+    "group_pose_jump": "标签组PnP结果发生突跳",
+}
+
+
+def pnp_rejection_zh(reason: str) -> str:
+    """Translate a PnP rejection code without hiding its machine value."""
+    return _REJECTION_ZH.get(str(reason), f"PnP拒绝：{reason}")
+
+
+def build_tag_quality_diagnostics(
+    tag_config: Mapping[str, Mapping[str, Any]],
+    qualities: Mapping[str, TagQuality],
+    rejections: Mapping[str, str],
+    reprojection_errors_px: Mapping[str, float],
+    *,
+    maximum_hamming: int,
+    minimum_decision_margin: float,
+    minimum_edge_pixels: float,
+    maximum_reprojection_error_px: float,
+) -> dict[str, dict[str, Any]]:
+    """Return compact per-tag values, failures and Chinese explanations."""
+    result: dict[str, dict[str, Any]] = {}
+    for role, config in tag_config.items():
+        quality = qualities.get(role)
+        rejection = rejections.get(role)
+        problems: list[str] = []
+        if quality is None:
+            problems.append("当前画面未检测到")
+        else:
+            if int(quality.hamming) > int(maximum_hamming):
+                problems.append(
+                    f"hamming={quality.hamming}，要求≤{maximum_hamming}"
+                )
+            if float(quality.decision_margin) < float(
+                minimum_decision_margin
+            ):
+                problems.append(
+                    "识别置信度"
+                    f"{quality.decision_margin:.1f}，要求≥"
+                    f"{minimum_decision_margin:.1f}"
+                )
+            if float(quality.edge_pixels) < float(minimum_edge_pixels):
+                problems.append(
+                    f"边长{quality.edge_pixels:.1f}px，要求≥"
+                    f"{minimum_edge_pixels:.1f}px"
+                )
+        reprojection = reprojection_errors_px.get(role)
+        if (
+            reprojection is not None
+            and float(reprojection) > float(maximum_reprojection_error_px)
+        ):
+            problems.append(
+                f"重投影误差{reprojection:.2f}px，要求≤"
+                f"{maximum_reprojection_error_px:.2f}px"
+            )
+        if rejection is not None and not problems:
+            problems.append(pnp_rejection_zh(rejection))
+
+        detection_valid = bool(
+            quality is not None
+            and int(quality.hamming) <= int(maximum_hamming)
+            and float(quality.decision_margin)
+            >= float(minimum_decision_margin)
+            and float(quality.edge_pixels) >= float(minimum_edge_pixels)
+        )
+        individual_valid = bool(
+            detection_valid
+            and rejection is None
+            and (
+                reprojection is None
+                or float(reprojection)
+                <= float(maximum_reprojection_error_px)
+            )
+        )
+        name_zh = ROLE_NAMES_ZH.get(role, role)
+        result[role] = {
+            "id": int(config["id"]),
+            "name_zh": name_zh,
+            "detected": quality is not None,
+            "individual_valid": individual_valid,
+            "hamming": None if quality is None else int(quality.hamming),
+            "decision_margin": (
+                None
+                if quality is None
+                else round(float(quality.decision_margin), 2)
+            ),
+            "edge_pixels": (
+                None
+                if quality is None
+                else round(float(quality.edge_pixels), 2)
+            ),
+            "reprojection_error_px": (
+                None
+                if reprojection is None
+                else round(float(reprojection), 3)
+            ),
+            "rejection": rejection,
+            "problems_zh": problems,
+            "summary_zh": (
+                f"{name_zh}正常"
+                if not problems
+                else f"{name_zh}：" + "；".join(problems)
+            ),
+        }
+    return result
+
+
+def status_guidance_zh(
+    state: str,
+    reason: str,
+    tag_diagnostics: Mapping[str, Mapping[str, Any]],
+) -> tuple[str, str]:
+    """Return a concise Chinese reason and the next practical action."""
+    reason_text = str(reason)
+    invalid_tags = [
+        item
+        for item in tag_diagnostics.values()
+        if not bool(item.get("individual_valid", False))
+    ]
+    tag_failure = (
+        "；".join(str(item["summary_zh"]) for item in invalid_tags)
+        if invalid_tags
+        else ""
+    )
+    if "invalid_tag_frame" in reason_text or (
+        state == "PREFLIGHT" and invalid_tags
+    ):
+        actions: list[str] = []
+        problem_text = "；".join(
+            str(problem)
+            for item in invalid_tags
+            for problem in item.get("problems_zh", [])
+        )
+        if "未检测到" in problem_text:
+            actions.append("调整遮挡或画面范围，让四张标签同时可见")
+        if "边长" in problem_text:
+            actions.append("将相机稍微靠近，建议标签边长稳定大于32px")
+        if "识别置信度" in problem_text:
+            actions.append("改善照明、对焦并避免标签反光")
+        if "hamming" in problem_text:
+            actions.append("清洁或重新打印标签，并检查对焦")
+        if "相机内参" in problem_text:
+            actions.append("确认CameraInfo话题正常")
+        if not actions:
+            actions.append("保持机械手不动，检查标签和相机画面")
+        actions.append("修正后调用resume继续")
+        return (
+            f"标签帧无效，无法采集。{tag_failure}",
+            "；".join(dict.fromkeys(actions)),
+        )
+
+    exact = {
+        "waiting_for_camera_tags_and_sdk": (
+            "正在等待相机、四张标签和灵巧手SDK就绪",
+            "保持设备连接，观察预检状态",
+        ),
+        "call_start": (
+            "预检已通过，等待开始标定",
+            "调用/g20_thumb_calibration/start",
+        ),
+        "call_confirm_root_full_range": (
+            "预检已通过，等待开始拇指根部全行程标定",
+            "调用/g20_thumb_calibration/start",
+        ),
+        "operator_pause": (
+            "操作员已暂停标定",
+            "确认安全后调用resume继续",
+        ),
+        "calibration_complete": (
+            "标定和随机复测已经完成",
+            "检查最终JSON中的quality.passed",
+        ),
+    }
+    if reason_text in exact:
+        return exact[reason_text]
+    if reason_text.startswith("collecting_detection_preflight"):
+        return "正在采集标签预检帧", "保持相机和机械手静止"
+    if reason_text.startswith("detection_hz_too_low"):
+        return (
+            f"AprilTag检测频率过低（{reason_text.split(':')[-1]}Hz）",
+            "关闭图像显示等额外订阅，检查相机和检测节点负载",
+        )
+    if reason_text.startswith("detection_rate_too_low"):
+        return "四张标签同时有效的比例过低", "检查遮挡、边长和照明"
+    if "not_stable" in reason_text:
+        return "标签三维中心尚未稳定，无法完成静态采集", (
+            "不要触碰相机和机械手，检查标签固定及关节抖动后调用resume"
+        )
+    if reason_text.startswith("continuous_sweep_insufficient_coverage"):
+        return "连续扫描采样覆盖不足", "降低标定速度或提高AprilTag检测频率后重扫"
+    if "synchronised_tag_state_timeout" in reason_text:
+        return (
+            "运动过程中连续3秒没有取得同时有效的标签帧和电机状态；"
+            "暂停后当前画面恢复正常也不会自动继续",
+            "确认四张标签全行程稳定可见，并检查/g20/cb_left_hand_state；"
+            "修正后调用resume继续，不要调用start",
+        )
+    if "timeout" in reason_text:
+        return f"等待超时：{reason_text}", "检查标签、同步状态和机械手是否到位"
+    if reason_text.startswith("pnp_trajectory"):
+        return f"三维轨迹质量检查未通过：{reason_text}", (
+            "检查标签固定、相机稳定和机械抖动"
+        )
+    if reason_text.startswith("root:continuous"):
+        return "正在连续采集拇指根部轨迹", "无需操作，等待该方向完成"
+    if reason_text.startswith("tip:continuous"):
+        return "正在连续采集拇指尖部轨迹", "无需操作，等待该方向完成"
+    return (
+        f"{STATE_NAMES_ZH.get(state, state)}：{reason_text}",
+        "根据英文reason机器码检查详细原因",
+    )
+
+
+def render_status_text_zh(
+    state_zh: str,
+    reason_zh: str,
+    action_zh: str,
+    tag_diagnostics: Mapping[str, Mapping[str, Any]],
+) -> str:
+    """Render a terminal-friendly multiline Chinese status message."""
+    lines = [
+        f"状态：{state_zh}",
+        f"原因：{reason_zh}",
+        f"建议：{action_zh}",
+        "标签：",
+    ]
+    for item in tag_diagnostics.values():
+        detected = bool(item.get("detected", False))
+        valid = bool(item.get("individual_valid", False))
+        if detected:
+            metrics = (
+                f"边长{float(item['edge_pixels']):.1f}px，"
+                f"置信度{float(item['decision_margin']):.1f}，"
+                f"hamming={int(item['hamming'])}"
+            )
+            reprojection = item.get("reprojection_error_px")
+            if reprojection is not None:
+                metrics += f"，重投影{float(reprojection):.2f}px"
+        else:
+            metrics = "未检测到"
+        problems = item.get("problems_zh", [])
+        problem_suffix = (
+            "" if not problems else "；" + "；".join(map(str, problems))
+        )
+        lines.append(
+            f"- {item['name_zh']}(ID {item['id']})："
+            f"{'正常' if valid else '异常'}，{metrics}{problem_suffix}"
+        )
+    return "\n".join(lines)

src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py

@@ -0,0 +1,979 @@
+"""Square AprilTag pose estimation with planar ambiguity tracking.
+
+The AprilTag detections contain accurately refined image corners.  This module
+uses OpenCV's IPPE square solver directly so the calibration node can inspect
+both planar PnP solutions instead of accepting an occasionally flipped TF
+pose.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass, replace
+from itertools import product
+import math
+from typing import Mapping, Sequence
+
+import cv2
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+
+@dataclass(frozen=True)
+class SquareTagPose:
+    """One tag-to-camera pose candidate returned by IPPE."""
+
+    quaternion_xyzw: tuple[float, float, float, float]
+    translation_xyz_m: tuple[float, float, float]
+    reprojection_error_px: float
+
+
+def _relative_pose(
+    parent: SquareTagPose,
+    child: SquareTagPose,
+) -> tuple[Rotation, np.ndarray]:
+    parent_rotation = Rotation.from_quat(parent.quaternion_xyzw)
+    child_rotation = Rotation.from_quat(child.quaternion_xyzw)
+    relative_rotation = parent_rotation.inv() * child_rotation
+    relative_translation = parent_rotation.inv().apply(
+        np.asarray(child.translation_xyz_m, dtype=float)
+        - np.asarray(parent.translation_xyz_m, dtype=float)
+    )
+    return relative_rotation, relative_translation
+
+
+def select_rigid_group_trajectory(
+    frames: Sequence[Mapping[str, Sequence[SquareTagPose]]],
+    *,
+    roles: Sequence[str],
+    fixed_pairs: Sequence[tuple[str, str]],
+    reprojection_scale_px: float,
+    rotation_scale_rad: float,
+    translation_scale_m: float,
+    pair_geometry: str = "pose",
+) -> tuple[
+    list[dict[str, SquareTagPose]],
+    dict[str, float | str],
+]:
+    """Resolve planar branches using geometry that should stay rigid.
+
+    Every possible branch combination in the first frame is treated as a
+    candidate rigid reference. For each such reference, every later frame
+    independently chooses the combination with the lowest reprojection plus
+    geometric-drift cost. ``pose`` compares relative rotation and translation;
+    ``distance`` compares only Euclidean centre distances and therefore does
+    not allow planar-PnP orientation jitter into centre-trajectory angles.
+    The globally cheapest reference and path win.
+    """
+    role_names = tuple(str(role) for role in roles)
+    pair_names = tuple((str(parent), str(child)) for parent, child in fixed_pairs)
+    if not frames:
+        raise ValueError("at least one PnP frame is required")
+    if len(set(role_names)) != len(role_names) or not role_names:
+        raise ValueError("roles must be non-empty and unique")
+    if any(
+        parent not in role_names or child not in role_names
+        for parent, child in pair_names
+    ):
+        raise ValueError("fixed_pairs must reference roles")
+    reprojection_scale = float(reprojection_scale_px)
+    rotation_scale = float(rotation_scale_rad)
+    translation_scale = float(translation_scale_m)
+    geometry_mode = str(pair_geometry)
+    if min(reprojection_scale, rotation_scale, translation_scale) <= 0.0:
+        raise ValueError("trajectory selection scales must be positive")
+    if geometry_mode not in {"pose", "distance"}:
+        raise ValueError("pair_geometry must be pose or distance")
+
+    combinations_by_frame: list[list[dict[str, SquareTagPose]]] = []
+    for frame in frames:
+        candidate_lists = [tuple(frame.get(role, ())) for role in role_names]
+        if any(not candidates for candidates in candidate_lists):
+            raise ValueError("every frame must contain every requested role")
+        combinations_by_frame.append(
+            [
+                dict(zip(role_names, combination))
+                for combination in product(*candidate_lists)
+            ]
+        )
+
+    best_total = float("inf")
+    best_path: list[dict[str, SquareTagPose]] | None = None
+
+    def emission(
+        combination: Mapping[str, SquareTagPose],
+        reference_pairs: Mapping[
+            tuple[str, str], tuple[Rotation, np.ndarray]
+        ],
+        reference_distances: Mapping[tuple[str, str], float],
+    ) -> tuple[float, float, float]:
+        reprojection_cost = sum(
+            pose.reprojection_error_px
+            for pose in combination.values()
+        ) / reprojection_scale
+        rotation_drifts: list[float] = []
+        translation_drifts: list[float] = []
+        distance_drifts: list[float] = []
+        for pair, (
+            reference_rotation,
+            reference_translation,
+        ) in reference_pairs.items():
+            rotation, translation = _relative_pose(
+                combination[pair[0]],
+                combination[pair[1]],
+            )
+            rotation_drifts.append(
+                float(
+                    (reference_rotation.inv() * rotation).magnitude()
+                )
+            )
+            translation_drifts.append(
+                float(
+                    np.linalg.norm(
+                        translation - reference_translation
+                    )
+                )
+            )
+            current_distance = float(
+                np.linalg.norm(
+                    np.asarray(
+                        combination[pair[1]].translation_xyz_m,
+                        dtype=float,
+                    )
+                    - np.asarray(
+                        combination[pair[0]].translation_xyz_m,
+                        dtype=float,
+                    )
+                )
+            )
+            distance_drifts.append(
+                abs(current_distance - reference_distances[pair])
+            )
+        if geometry_mode == "distance":
+            geometry_cost = sum(distance_drifts) / translation_scale
+        else:
+            geometry_cost = (
+                sum(rotation_drifts) / rotation_scale
+                + sum(translation_drifts) / translation_scale
+            )
+        return (
+            reprojection_cost + geometry_cost,
+            max(rotation_drifts, default=0.0),
+            (
+                max(distance_drifts, default=0.0)
+                if geometry_mode == "distance"
+                else max(translation_drifts, default=0.0)
+            ),
+        )
+
+    def transition_cost(
+        previous: Mapping[str, SquareTagPose],
+        current: Mapping[str, SquareTagPose],
+    ) -> float:
+        rotation_motion = sum(
+            rotation_distance_rad(
+                previous[role].quaternion_xyzw,
+                current[role].quaternion_xyzw,
+            )
+            for role in role_names
+        )
+        translation_motion = sum(
+            float(
+                np.linalg.norm(
+                    np.asarray(current[role].translation_xyz_m)
+                    - np.asarray(previous[role].translation_xyz_m)
+                )
+            )
+            for role in role_names
+        )
+        if geometry_mode == "distance":
+            return translation_motion / translation_scale
+        return (
+            rotation_motion / rotation_scale
+            + translation_motion / translation_scale
+        )
+
+    for reference_index, reference_combination in enumerate(
+        combinations_by_frame[0]
+    ):
+        reference_pairs = {
+            pair: _relative_pose(
+                reference_combination[pair[0]],
+                reference_combination[pair[1]],
+            )
+            for pair in pair_names
+        }
+        reference_distances = {
+            pair: float(
+                np.linalg.norm(
+                    np.asarray(
+                        reference_combination[pair[1]].translation_xyz_m,
+                        dtype=float,
+                    )
+                    - np.asarray(
+                        reference_combination[pair[0]].translation_xyz_m,
+                        dtype=float,
+                    )
+                )
+            )
+            for pair in pair_names
+        }
+        first_emission = emission(
+            reference_combination,
+            reference_pairs,
+            reference_distances,
+        )
+        previous_costs = np.full(
+            len(combinations_by_frame[0]),
+            np.inf,
+            dtype=float,
+        )
+        previous_costs[reference_index] = first_emission[0]
+        back_pointers: list[list[int]] = []
+        for frame_index in range(1, len(combinations_by_frame)):
+            previous_combinations = combinations_by_frame[frame_index - 1]
+            combinations = combinations_by_frame[frame_index]
+            frame_emissions = [
+                emission(
+                    combination,
+                    reference_pairs,
+                    reference_distances,
+                )
+                for combination in combinations
+            ]
+            current_costs = np.full(len(combinations), np.inf, dtype=float)
+            frame_back_pointers: list[int] = []
+            for current_index, combination in enumerate(combinations):
+                transition_costs = [
+                    previous_costs[previous_index]
+                    + transition_cost(
+                        previous_combination,
+                        combination,
+                    )
+                    for previous_index, previous_combination in enumerate(
+                        previous_combinations
+                    )
+                ]
+                best_previous = int(np.argmin(transition_costs))
+                frame_back_pointers.append(best_previous)
+                current_costs[current_index] = (
+                    transition_costs[best_previous]
+                    + frame_emissions[current_index][0]
+                )
+            back_pointers.append(frame_back_pointers)
+            previous_costs = current_costs
+
+        final_index = int(np.argmin(previous_costs))
+        total = float(previous_costs[final_index])
+        path_indices = [final_index]
+        for frame_back_pointers in reversed(back_pointers):
+            path_indices.append(
+                frame_back_pointers[path_indices[-1]]
+            )
+        path_indices.reverse()
+        path = [
+            combinations[index]
+            for combinations, index in zip(
+                combinations_by_frame,
+                path_indices,
+            )
+        ]
+        if total < best_total:
+            best_total = total
+            best_path = path
+
+    if best_path is None:
+        raise RuntimeError("trajectory branch selection produced no path")
+
+    # The marker-to-marker mounting transforms are unknown, so the rigid
+    # reference must be estimated from the complete sweep.  Using frame zero
+    # as both the optimisation seed and the reported quality reference made
+    # one noisy endpoint frame look like drift in every other frame.  A
+    # rotation medoid and component-wise translation median are insensitive
+    # to that endpoint noise while still exposing a persistent mirror branch.
+    robust_reference_pairs: dict[
+        tuple[str, str], tuple[Rotation, np.ndarray]
+    ] = {}
+    for pair in pair_names:
+        pair_poses = [
+            _relative_pose(frame[pair[0]], frame[pair[1]])
+            for frame in best_path
+        ]
+        pair_rotations = [pose[0] for pose in pair_poses]
+        angular_costs = np.asarray(
+            [
+                sum(
+                    float((candidate.inv() * other).magnitude())
+                    for other in pair_rotations
+                )
+                for candidate in pair_rotations
+            ],
+            dtype=float,
+        )
+        rotation_medoid = pair_rotations[int(np.argmin(angular_costs))]
+        translation_median = np.median(
+            np.asarray([pose[1] for pose in pair_poses], dtype=float),
+            axis=0,
+        )
+        robust_reference_pairs[pair] = (
+            rotation_medoid,
+            translation_median,
+        )
+
+    rotation_drifts_by_frame: list[float] = []
+    translation_drifts_by_frame: list[float] = []
+    pair_distances_by_pair = {
+        pair: np.asarray(
+            [
+                np.linalg.norm(
+                    np.asarray(frame[pair[1]].translation_xyz_m, dtype=float)
+                    - np.asarray(
+                        frame[pair[0]].translation_xyz_m, dtype=float
+                    )
+                )
+                for frame in best_path
+            ],
+            dtype=float,
+        )
+        for pair in pair_names
+    }
+    robust_pair_distances = {
+        pair: float(np.median(distances))
+        for pair, distances in pair_distances_by_pair.items()
+    }
+    distance_drifts_by_frame: list[float] = []
+    for frame in best_path:
+        frame_rotation_drifts: list[float] = []
+        frame_translation_drifts: list[float] = []
+        frame_distance_drifts: list[float] = []
+        for pair, (
+            reference_rotation,
+            reference_translation,
+        ) in robust_reference_pairs.items():
+            rotation, translation = _relative_pose(
+                frame[pair[0]], frame[pair[1]]
+            )
+            frame_rotation_drifts.append(
+                float((reference_rotation.inv() * rotation).magnitude())
+            )
+            frame_translation_drifts.append(
+                float(np.linalg.norm(translation - reference_translation))
+            )
+            distance = float(
+                np.linalg.norm(
+                    np.asarray(frame[pair[1]].translation_xyz_m, dtype=float)
+                    - np.asarray(
+                        frame[pair[0]].translation_xyz_m, dtype=float
+                    )
+                )
+            )
+            frame_distance_drifts.append(
+                abs(distance - robust_pair_distances[pair])
+            )
+        rotation_drifts_by_frame.append(
+            max(frame_rotation_drifts, default=0.0)
+        )
+        translation_drifts_by_frame.append(
+            max(frame_translation_drifts, default=0.0)
+        )
+        distance_drifts_by_frame.append(
+            max(frame_distance_drifts, default=0.0)
+        )
+
+    rotation_drifts = np.asarray(rotation_drifts_by_frame, dtype=float)
+    translation_drifts = np.asarray(
+        translation_drifts_by_frame, dtype=float
+    )
+    distance_drifts = np.asarray(distance_drifts_by_frame, dtype=float)
+    return best_path, {
+        "total_cost": float(best_total),
+        "pair_geometry": geometry_mode,
+        "maximum_pair_rotation_drift_rad": float(
+            np.max(rotation_drifts, initial=0.0)
+        ),
+        "p95_pair_rotation_drift_rad": float(
+            np.percentile(rotation_drifts, 95.0)
+        ),
+        "median_pair_rotation_drift_rad": float(
+            np.median(rotation_drifts)
+        ),
+        "maximum_pair_translation_drift_m": float(
+            np.max(translation_drifts, initial=0.0)
+        ),
+        "p95_pair_translation_drift_m": float(
+            np.percentile(translation_drifts, 95.0)
+        ),
+        "maximum_pair_distance_drift_m": float(
+            np.max(distance_drifts, initial=0.0)
+        ),
+        "p95_pair_distance_drift_m": float(
+            np.percentile(distance_drifts, 95.0)
+        ),
+        "median_pair_distance_drift_m": float(
+            np.median(distance_drifts)
+        ),
+    }
+
+
+def _as_camera_matrix(camera_matrix: Sequence[Sequence[float]]) -> np.ndarray:
+    matrix = np.asarray(camera_matrix, dtype=np.float64)
+    if matrix.shape != (3, 3):
+        raise ValueError("camera_matrix must have shape (3, 3)")
+    if not np.all(np.isfinite(matrix)):
+        raise ValueError("camera_matrix must be finite")
+    if matrix[0, 0] <= 0.0 or matrix[1, 1] <= 0.0:
+        raise ValueError("camera focal lengths must be positive")
+    return matrix
+
+
+def square_object_points(tag_size_m: float) -> np.ndarray:
+    """Return IPPE-square points matching apriltag_msgs corner order.
+
+    ``apriltag_ros`` reports bottom-left, bottom-right, top-right, top-left.
+    OpenCV's ``SOLVEPNP_IPPE_SQUARE`` requires the same physical corners in
+    the order below.
+    """
+    size = float(tag_size_m)
+    if not math.isfinite(size) or size <= 0.0:
+        raise ValueError("tag_size_m must be finite and positive")
+    half = size / 2.0
+    return np.asarray(
+        [
+            [-half, half, 0.0],
+            [half, half, 0.0],
+            [half, -half, 0.0],
+            [-half, -half, 0.0],
+        ],
+        dtype=np.float64,
+    )
+
+
+def solve_square_tag_ippe(
+    corners_xy: Sequence[Sequence[float]],
+    *,
+    tag_size_m: float,
+    camera_matrix: Sequence[Sequence[float]],
+) -> list[SquareTagPose]:
+    """Return every finite, positive-depth IPPE pose for one square tag."""
+    image_points = np.asarray(corners_xy, dtype=np.float64)
+    if image_points.shape != (4, 2):
+        raise ValueError("corners_xy must have shape (4, 2)")
+    if not np.all(np.isfinite(image_points)):
+        raise ValueError("corners_xy must be finite")
+    intrinsic = _as_camera_matrix(camera_matrix)
+    object_points = square_object_points(tag_size_m)
+    distortion = np.zeros((4, 1), dtype=np.float64)
+
+    solved, rotation_vectors, translations, _ = cv2.solvePnPGeneric(
+        object_points,
+        image_points,
+        intrinsic,
+        distortion,
+        flags=cv2.SOLVEPNP_IPPE_SQUARE,
+    )
+    if not solved:
+        return []
+
+    candidates: list[SquareTagPose] = []
+    for rotation_vector, translation in zip(rotation_vectors, translations):
+        rotation_matrix, _ = cv2.Rodrigues(rotation_vector)
+        translation_vector = np.asarray(translation, dtype=float).reshape(3)
+        camera_points = (
+            rotation_matrix @ object_points.T
+            + translation_vector.reshape(3, 1)
+        ).T
+        if np.min(camera_points[:, 2]) <= 0.0:
+            continue
+        projected, _ = cv2.projectPoints(
+            object_points,
+            rotation_vector,
+            translation_vector,
+            intrinsic,
+            distortion,
+        )
+        residual = projected.reshape(4, 2) - image_points
+        reprojection_error = float(
+            np.sqrt(np.mean(np.sum(residual * residual, axis=1)))
+        )
+        quaternion = Rotation.from_matrix(rotation_matrix).as_quat()
+        if not (
+            np.all(np.isfinite(quaternion))
+            and np.all(np.isfinite(translation_vector))
+            and math.isfinite(reprojection_error)
+        ):
+            continue
+        candidates.append(
+            SquareTagPose(
+                quaternion_xyzw=tuple(float(value) for value in quaternion),
+                translation_xyz_m=tuple(
+                    float(value) for value in translation_vector
+                ),
+                reprojection_error_px=reprojection_error,
+            )
+        )
+    return candidates
+
+
+def rotation_distance_rad(
+    first_xyzw: Sequence[float],
+    second_xyzw: Sequence[float],
+) -> float:
+    first = Rotation.from_quat(np.asarray(first_xyzw, dtype=float))
+    second = Rotation.from_quat(np.asarray(second_xyzw, dtype=float))
+    return float((first.inv() * second).magnitude())
+
+
+def select_continuous_pose(
+    candidates: Sequence[SquareTagPose],
+    *,
+    previous: SquareTagPose | None,
+    maximum_reprojection_error_px: float,
+    reprojection_tie_px: float,
+    maximum_pose_jump_rad: float,
+    maximum_translation_jump_m: float,
+    maximum_tag_tilt_rad: float,
+) -> tuple[SquareTagPose | None, str]:
+    """Select the best IPPE branch using image fit and temporal continuity."""
+    maximum_error = float(maximum_reprojection_error_px)
+    tie_error = float(reprojection_tie_px)
+    maximum_rotation = float(maximum_pose_jump_rad)
+    maximum_translation = float(maximum_translation_jump_m)
+    maximum_tilt = float(maximum_tag_tilt_rad)
+    if min(
+        maximum_error,
+        maximum_rotation,
+        maximum_translation,
+        maximum_tilt,
+    ) <= 0.0:
+        raise ValueError("PnP selection thresholds must be positive")
+    if tie_error < 0.0:
+        raise ValueError("reprojection_tie_px must be non-negative")
+
+    eligible: list[SquareTagPose] = []
+    for candidate in candidates:
+        if candidate.reprojection_error_px > maximum_error:
+            continue
+        normal = Rotation.from_quat(candidate.quaternion_xyzw).as_matrix()[:, 2]
+        tilt = math.acos(float(np.clip(abs(normal[2]), 0.0, 1.0)))
+        if tilt > maximum_tilt:
+            continue
+        eligible.append(candidate)
+    if not eligible:
+        return None, "no_pose_within_reprojection_or_tilt_limit"
+
+    eligible.sort(key=lambda item: item.reprojection_error_px)
+    best = eligible[0]
+    if previous is None:
+        return best, ""
+
+    # Temporal continuity must only break a genuine planar-PnP tie.  The old
+    # implementation normalised reprojection error by the permissive 1.5 px
+    # rejection limit, which allowed a stale mirror branch at 0.25 px to beat
+    # the true branch at e.g. 0.05 px merely because it was closer to the
+    # preceding (already wrong) pose.  Once one IPPE solution has a meaningful
+    # image-fit advantage, trust it and allow the tracker to leave the stale
+    # branch even if that correction is a large pose jump.
+    competitive = [
+        candidate
+        for candidate in eligible
+        if candidate.reprojection_error_px
+        <= best.reprojection_error_px + tie_error
+    ]
+    if len(competitive) == 1:
+        return best, ""
+
+    previous_translation = np.asarray(previous.translation_xyz_m, dtype=float)
+    scored: list[tuple[float, SquareTagPose]] = []
+    for candidate in competitive:
+        rotation_jump = rotation_distance_rad(
+            previous.quaternion_xyzw,
+            candidate.quaternion_xyzw,
+        )
+        translation_jump = float(
+            np.linalg.norm(
+                np.asarray(candidate.translation_xyz_m, dtype=float)
+                - previous_translation
+            )
+        )
+        if (
+            rotation_jump > maximum_rotation
+            or translation_jump > maximum_translation
+        ):
+            continue
+        score = (
+            (
+                candidate.reprojection_error_px
+                - best.reprojection_error_px
+            )
+            / max(tie_error, np.finfo(float).eps)
+            + rotation_jump / maximum_rotation
+            + translation_jump / maximum_translation
+        )
+        scored.append((float(score), candidate))
+    if not scored:
+        return None, "pose_jump"
+
+    selected = min(scored, key=lambda item: item[0])[1]
+    previous_quaternion = np.asarray(previous.quaternion_xyzw, dtype=float)
+    selected_quaternion = np.asarray(selected.quaternion_xyzw, dtype=float)
+    if float(np.dot(previous_quaternion, selected_quaternion)) < 0.0:
+        selected = replace(
+            selected,
+            quaternion_xyzw=tuple(
+                float(value) for value in -selected_quaternion
+            ),
+        )
+    return selected, ""
+
+
+class SquareTagPoseTracker:
+    """Maintain the selected planar-PnP branch independently for each tag."""
+
+    def __init__(
+        self,
+        *,
+        maximum_reprojection_error_px: float,
+        reprojection_tie_px: float,
+        maximum_pose_jump_rad: float,
+        maximum_translation_jump_m: float,
+        maximum_tag_tilt_rad: float,
+        reset_after_seconds: float,
+    ) -> None:
+        self.maximum_reprojection_error_px = float(
+            maximum_reprojection_error_px
+        )
+        self.reprojection_tie_px = float(reprojection_tie_px)
+        self.maximum_pose_jump_rad = float(maximum_pose_jump_rad)
+        self.maximum_translation_jump_m = float(maximum_translation_jump_m)
+        self.maximum_tag_tilt_rad = float(maximum_tag_tilt_rad)
+        self.reset_after_ns = int(float(reset_after_seconds) * 1_000_000_000)
+        if self.reset_after_ns <= 0:
+            raise ValueError("reset_after_seconds must be positive")
+        self._previous: dict[str, tuple[int, SquareTagPose]] = {}
+        self.last_candidates_by_role: dict[
+            str, tuple[SquareTagPose, ...]
+        ] = {}
+        self.branch_correction_counts: dict[str, int] = {}
+
+    def reset(self) -> None:
+        self._previous.clear()
+        self.last_candidates_by_role.clear()
+        self.branch_correction_counts.clear()
+
+    def estimate(
+        self,
+        role: str,
+        corners_xy: Sequence[Sequence[float]],
+        *,
+        tag_size_m: float,
+        camera_matrix: Sequence[Sequence[float]],
+        stamp_ns: int,
+        reprojection_tie_px: float | None = None,
+    ) -> tuple[SquareTagPose | None, str]:
+        try:
+            candidates = solve_square_tag_ippe(
+                corners_xy,
+                tag_size_m=tag_size_m,
+                camera_matrix=camera_matrix,
+            )
+        except (ValueError, cv2.error):
+            self.last_candidates_by_role[str(role)] = ()
+            return None, "pnp_solve_failed"
+        if not candidates:
+            self.last_candidates_by_role[str(role)] = ()
+            return None, "pnp_solve_failed"
+        usable_candidates: list[SquareTagPose] = []
+        for candidate in candidates:
+            normal = Rotation.from_quat(
+                candidate.quaternion_xyzw
+            ).as_matrix()[:, 2]
+            tilt = math.acos(
+                float(np.clip(abs(normal[2]), 0.0, 1.0))
+            )
+            if (
+                candidate.reprojection_error_px
+                <= self.maximum_reprojection_error_px
+                and tilt <= self.maximum_tag_tilt_rad
+            ):
+                usable_candidates.append(candidate)
+        self.last_candidates_by_role[str(role)] = tuple(
+            usable_candidates
+        )
+        if not usable_candidates:
+            return None, "no_pose_within_reprojection_or_tilt_limit"
+
+        previous_record = self._previous.get(str(role))
+        previous: SquareTagPose | None = None
+        if previous_record is not None:
+            previous_stamp, previous_pose = previous_record
+            elapsed = int(stamp_ns) - previous_stamp
+            if 0 <= elapsed <= self.reset_after_ns:
+                previous = previous_pose
+
+        selected, reason = select_continuous_pose(
+            usable_candidates,
+            previous=previous,
+            maximum_reprojection_error_px=(
+                self.maximum_reprojection_error_px
+            ),
+            reprojection_tie_px=(
+                self.reprojection_tie_px
+                if reprojection_tie_px is None
+                else float(reprojection_tie_px)
+            ),
+            maximum_pose_jump_rad=self.maximum_pose_jump_rad,
+            maximum_translation_jump_m=self.maximum_translation_jump_m,
+            maximum_tag_tilt_rad=self.maximum_tag_tilt_rad,
+        )
+        if selected is not None:
+            if previous is not None:
+                rotation_jump = rotation_distance_rad(
+                    previous.quaternion_xyzw,
+                    selected.quaternion_xyzw,
+                )
+                translation_jump = float(
+                    np.linalg.norm(
+                        np.asarray(selected.translation_xyz_m, dtype=float)
+                        - np.asarray(
+                            previous.translation_xyz_m,
+                            dtype=float,
+                        )
+                    )
+                )
+                if (
+                    rotation_jump > self.maximum_pose_jump_rad
+                    or translation_jump
+                    > self.maximum_translation_jump_m
+                ):
+                    key = str(role)
+                    self.branch_correction_counts[key] = (
+                        self.branch_correction_counts.get(key, 0) + 1
+                    )
+            self._previous[str(role)] = (int(stamp_ns), selected)
+        return selected, reason
+
+
+class SquareTagGroupPoseTracker:
+    """Choose all tag branches together using thumb-chain continuity.
+
+    A 30 px planar tag has two IPPE solutions whose reprojection errors can
+    exchange order from one frame to the next.  Tracking each tag
+    independently can therefore choose an incompatible pair for a relative
+    joint such as T4->T5.  This tracker enumerates the small Cartesian product
+    (at most 2**4 combinations) and favours the combination that keeps both
+    the camera poses and all adjacent relative poses continuous.
+    """
+
+    def __init__(
+        self,
+        *,
+        roles: Sequence[str],
+        adjacent_pairs: Sequence[tuple[str, str]],
+        maximum_pose_jump_rad: float,
+        maximum_translation_jump_m: float,
+        relative_rotation_scale_rad: float,
+        relative_translation_scale_m: float,
+        reprojection_scale_px: float,
+        reprojection_weight: float,
+        reset_after_seconds: float,
+    ) -> None:
+        self.roles = tuple(str(role) for role in roles)
+        self.adjacent_pairs = tuple(
+            (str(parent), str(child))
+            for parent, child in adjacent_pairs
+        )
+        if not self.roles or len(set(self.roles)) != len(self.roles):
+            raise ValueError("roles must be non-empty and unique")
+        if any(
+            parent not in self.roles or child not in self.roles
+            for parent, child in self.adjacent_pairs
+        ):
+            raise ValueError("adjacent_pairs must reference roles")
+        self.maximum_pose_jump_rad = float(maximum_pose_jump_rad)
+        self.maximum_translation_jump_m = float(
+            maximum_translation_jump_m
+        )
+        self.relative_rotation_scale_rad = float(
+            relative_rotation_scale_rad
+        )
+        self.relative_translation_scale_m = float(
+            relative_translation_scale_m
+        )
+        self.reprojection_scale_px = float(reprojection_scale_px)
+        self.reprojection_weight = float(reprojection_weight)
+        reset_seconds = float(reset_after_seconds)
+        if min(
+            self.maximum_pose_jump_rad,
+            self.maximum_translation_jump_m,
+            self.relative_rotation_scale_rad,
+            self.relative_translation_scale_m,
+            self.reprojection_scale_px,
+            reset_seconds,
+        ) <= 0.0:
+            raise ValueError("group tracking scales must be positive")
+        if self.reprojection_weight < 0.0:
+            raise ValueError("reprojection_weight must be non-negative")
+        self.reset_after_ns = int(reset_seconds * 1_000_000_000)
+        self._previous: dict[str, SquareTagPose] = {}
+        self._previous_stamp_ns: int | None = None
+        self.branch_correction_counts: dict[str, int] = {}
+
+    def reset(self) -> None:
+        self._previous.clear()
+        self._previous_stamp_ns = None
+        self.branch_correction_counts.clear()
+
+    def select(
+        self,
+        candidates_by_role: Mapping[str, Sequence[SquareTagPose]],
+        *,
+        stamp_ns: int,
+    ) -> tuple[dict[str, SquareTagPose] | None, str]:
+        """Return one mutually consistent pose for every configured role."""
+        candidate_lists = [
+            tuple(candidates_by_role.get(role, ()))
+            for role in self.roles
+        ]
+        if any(not candidates for candidates in candidate_lists):
+            return None, "group_missing_pose_candidates"
+
+        combinations = [
+            dict(zip(self.roles, combination))
+            for combination in product(*candidate_lists)
+        ]
+        minimum_errors = {
+            role: min(
+                candidate.reprojection_error_px
+                for candidate in candidates
+            )
+            for role, candidates in zip(self.roles, candidate_lists)
+        }
+        stamp = int(stamp_ns)
+        previous_is_fresh = (
+            self._previous_stamp_ns is not None
+            and 0 <= stamp - self._previous_stamp_ns
+            <= self.reset_after_ns
+            and set(self._previous) == set(self.roles)
+        )
+
+        if not previous_is_fresh:
+            selected = min(
+                combinations,
+                key=lambda combination: sum(
+                    pose.reprojection_error_px
+                    for pose in combination.values()
+                ),
+            )
+        else:
+            previous_relative = {
+                pair: _relative_pose(
+                    self._previous[pair[0]],
+                    self._previous[pair[1]],
+                )
+                for pair in self.adjacent_pairs
+            }
+            scored: list[tuple[float, dict[str, SquareTagPose]]] = []
+            for combination in combinations:
+                absolute_rotation_motion = 0.0
+                absolute_translation_motion = 0.0
+                rejected = False
+                for role in self.roles:
+                    rotation_motion = rotation_distance_rad(
+                        self._previous[role].quaternion_xyzw,
+                        combination[role].quaternion_xyzw,
+                    )
+                    translation_motion = float(
+                        np.linalg.norm(
+                            np.asarray(
+                                combination[role].translation_xyz_m,
+                                dtype=float,
+                            )
+                            - np.asarray(
+                                self._previous[role].translation_xyz_m,
+                                dtype=float,
+                            )
+                        )
+                    )
+                    if (
+                        rotation_motion > self.maximum_pose_jump_rad
+                        or translation_motion
+                        > self.maximum_translation_jump_m
+                    ):
+                        rejected = True
+                        break
+                    absolute_rotation_motion += rotation_motion
+                    absolute_translation_motion += translation_motion
+                if rejected:
+                    continue
+
+                relative_rotation_motion = 0.0
+                relative_translation_motion = 0.0
+                for pair in self.adjacent_pairs:
+                    rotation, translation = _relative_pose(
+                        combination[pair[0]],
+                        combination[pair[1]],
+                    )
+                    old_rotation, old_translation = previous_relative[pair]
+                    relative_rotation_motion += float(
+                        (old_rotation.inv() * rotation).magnitude()
+                    )
+                    relative_translation_motion += float(
+                        np.linalg.norm(translation - old_translation)
+                    )
+
+                reprojection_penalty = sum(
+                    max(
+                        0.0,
+                        combination[role].reprojection_error_px
+                        - minimum_errors[role],
+                    )
+                    for role in self.roles
+                ) / self.reprojection_scale_px
+                score = (
+                    absolute_rotation_motion
+                    / self.maximum_pose_jump_rad
+                    + absolute_translation_motion
+                    / self.maximum_translation_jump_m
+                    + relative_rotation_motion
+                    / self.relative_rotation_scale_rad
+                    + relative_translation_motion
+                    / self.relative_translation_scale_m
+                    + self.reprojection_weight * reprojection_penalty
+                )
+                scored.append((float(score), combination))
+
+            if not scored:
+                return None, "group_pose_jump"
+            selected = min(scored, key=lambda item: item[0])[1]
+
+        aligned: dict[str, SquareTagPose] = {}
+        for role in self.roles:
+            pose = selected[role]
+            if previous_is_fresh:
+                old_quaternion = np.asarray(
+                    self._previous[role].quaternion_xyzw,
+                    dtype=float,
+                )
+                quaternion = np.asarray(
+                    pose.quaternion_xyzw,
+                    dtype=float,
+                )
+                if float(np.dot(old_quaternion, quaternion)) < 0.0:
+                    pose = replace(
+                        pose,
+                        quaternion_xyzw=tuple(
+                            float(value) for value in -quaternion
+                        ),
+                    )
+            best_reprojection = min(
+                candidate_lists[self.roles.index(role)],
+                key=lambda candidate: candidate.reprojection_error_px,
+            )
+            if pose != best_reprojection:
+                self.branch_correction_counts[role] = (
+                    self.branch_correction_counts.get(role, 0) + 1
+                )
+            aligned[role] = pose
+
+        self._previous = aligned
+        self._previous_stamp_ns = stamp
+        return dict(aligned), ""

src/g20_thumb_apriltag_calibration/launch/front_thumb_calibration.launch.py

@@ -244,6 +244,10 @@ def _launch_stack(context):
                     LaunchConfiguration("continuous_motion_mode"),
                     value_type=str,
                 ),
+                "angle_estimation_mode": ParameterValue(
+                    LaunchConfiguration("angle_estimation_mode"),
+                    value_type=str,
+                ),
                 "camera_serial_number": LaunchConfiguration("camera_serial_number"),
                 "rosbag_path": str(bag_path),
                 "camera_info_topic": processed_camera_info_topic,
@@ -330,6 +334,10 @@ def generate_launch_description() -> LaunchDescription:
             DeclareLaunchArgument(
                 "continuous_motion_mode", default_value="endpoint"
             ),
+            DeclareLaunchArgument(
+                "angle_estimation_mode",
+                default_value="trajectory_center_3d",
+            ),
             DeclareLaunchArgument("apriltag_decimate", default_value="1.5"),
             DeclareLaunchArgument("commands_enabled", default_value="true"),
             DeclareLaunchArgument("start_camera", default_value="true"),

src/g20_thumb_apriltag_calibration/test/test_acquisition.py

@@ -3,6 +3,7 @@ from __future__ import annotations
 from dataclasses import replace
 
 import numpy as np
+import pytest
 from scipy.spatial.transform import Rotation
 
 from g20_thumb_apriltag_calibration.acquisition import (
@@ -36,6 +37,22 @@ def _observation(index: int, angle_rad: float = 0.0) -> Observation:
     )
 
 
+def _point_observation(
+    index: int,
+    *,
+    angle_rad: float = 0.0,
+    motor_value: float = 100.0,
+) -> Observation:
+    state = [255.0] * 20
+    state[0] = float(motor_value)
+    return replace(
+        _observation(index, angle_rad),
+        state_u8=tuple(state),
+        state_stamp_ns=index,
+        state_sync_error_ns=5_000_000,
+    )
+
+
 def test_stable_window_then_thirty_frame_capture() -> None:
     collector = PointCollector(
         stable_frames=15,
@@ -59,6 +76,119 @@ def test_stable_window_then_thirty_frame_capture() -> None:
     assert len(result["state_u8_median"]) == 20
 
 
+def test_translation_stability_ignores_planar_orientation_jitter() -> None:
+    collector = PointCollector(
+        stable_frames=3,
+        capture_frames=3,
+        minimum_settle_seconds=0.0,
+        stability_mode="translation",
+        maximum_stable_translation_spread_m=0.001,
+    )
+    collector.start(0.0)
+    result = None
+    for index in range(6):
+        observation = replace(
+            _observation(index, angle_rad=np.deg2rad(10.0 * index)),
+            tag_translation_xyz_m={
+                "t0": (0.00, 0.00, 0.50),
+                "t3": (0.03, 0.00, 0.50),
+                "t4": (0.06, 0.00, 0.50),
+                "t5": (0.09, 0.00, 0.50),
+            },
+        )
+        result = collector.add(observation, index / 30.0)
+    assert result is not None
+    assert collector.state == "complete"
+    assert collector.stable_spread_rad == {}
+    assert max(collector.stable_spread_m.values()) == 0.0
+
+
+def test_point_capture_waits_for_synchronised_target_state() -> None:
+    collector = PointCollector(
+        stable_frames=3,
+        capture_frames=3,
+        minimum_settle_seconds=0.0,
+        maximum_stable_spread_rad=np.deg2rad(0.3),
+    )
+    collector.start(
+        0.0,
+        required_state_index=0,
+        required_state_u8=100.0,
+        maximum_state_error_u8=2.0,
+    )
+    for index in range(3):
+        collector.add(
+            _point_observation(index, motor_value=108.0),
+            index / 30.0,
+        )
+    assert collector.state == "settling"
+    assert collector.stable_frames_seen == 0
+    assert collector.reason == "motor_position_out_of_tolerance"
+
+    result = None
+    for index in range(3, 9):
+        result = collector.add(
+            _point_observation(index, motor_value=101.0),
+            index / 30.0,
+        )
+    assert result is not None
+    assert collector.state == "complete"
+    assert result["state_u8_median"][0] == 101.0
+
+
+def test_unstable_capture_frames_are_not_aggregated() -> None:
+    collector = PointCollector(
+        stable_frames=3,
+        capture_frames=3,
+        minimum_settle_seconds=0.0,
+        maximum_stable_spread_rad=np.deg2rad(0.3),
+    )
+    collector.start(0.0)
+    for index in range(3):
+        collector.add(_observation(index), index / 30.0)
+    assert collector.state == "capturing"
+
+    result = None
+    # The capture block is internally stable but belongs to a different
+    # planar-PnP branch than the preceding stable window.
+    for index, angle_deg in enumerate((25.0, 25.0, 25.0), start=3):
+        result = collector.add(
+            _observation(index, np.deg2rad(angle_deg)),
+            index / 30.0,
+        )
+    assert result is None
+    assert collector.state == "settling"
+    assert collector.capture_frames_seen == 0
+    assert collector.reason.endswith("capture_not_stable")
+
+
+def test_position_drift_during_capture_restarts_settling() -> None:
+    collector = PointCollector(
+        stable_frames=3,
+        capture_frames=3,
+        minimum_settle_seconds=0.0,
+    )
+    collector.start(
+        0.0,
+        required_state_index=0,
+        required_state_u8=100.0,
+        maximum_state_error_u8=2.0,
+    )
+    for index in range(3):
+        collector.add(
+            _point_observation(index, motor_value=100.0),
+            index / 30.0,
+        )
+    assert collector.state == "capturing"
+    collector.add(
+        _point_observation(3, motor_value=108.0),
+        0.1,
+    )
+    assert collector.state == "settling"
+    assert collector.capture_frames_seen == 0
+    assert collector.reason == "motor_position_out_of_tolerance"
+
+
 def test_missing_tags_eventually_pauses_point_collector() -> None:
     collector = PointCollector(settle_timeout_seconds=5.0)
     collector.start(10.0)
@@ -75,6 +205,7 @@ def test_unstable_window_does_not_enter_capture() -> None:
         collector.add(_observation(index, angle), index / 30.0)
     assert collector.state == "settling"
     assert collector.reason.endswith("not_stable")
+    assert max(collector.stable_spread_rad.values()) > np.deg2rad(0.3)
 
 
 def test_isolated_invalid_frame_is_skipped_without_losing_valid_window() -> None:
@@ -123,6 +254,59 @@ def test_aggregate_keeps_worst_tag_quality() -> None:
     }
 
 
+def test_aggregate_keeps_median_tag_centres() -> None:
+    observations = [
+        replace(
+            _observation(index),
+            tag_translation_xyz_m={
+                role: (0.01 * index, 0.02, 0.50)
+                for role in ("t0", "t3", "t4", "t5")
+            },
+        )
+        for index in range(3)
+    ]
+    aggregate = aggregate_observations(observations)
+    assert aggregate["tag_translation_xyz_m"]["t4"] == pytest.approx(
+        [0.01, 0.02, 0.50]
+    )
+
+
+def test_pnp_reprojection_error_is_filtered_and_aggregated() -> None:
+    good = TagQuality(
+        hamming=0,
+        decision_margin=50.0,
+        edge_pixels=40.0,
+        reprojection_error_px=0.4,
+    )
+    bad = replace(good, reprojection_error_px=1.6)
+    thresholds = {
+        "maximum_hamming": 0,
+        "minimum_decision_margin": 30.0,
+        "minimum_edge_pixels": 30.0,
+        "maximum_reprojection_error_px": 1.5,
+    }
+    assert tag_quality_is_valid(good, **thresholds)
+    assert not tag_quality_is_valid(bad, **thresholds)
+
+    observations = [
+        replace(
+            _observation(index),
+            tag_quality={
+                role: replace(good, reprojection_error_px=error)
+                for role in ("t0", "t3", "t4", "t5")
+            },
+        )
+        for index, error in enumerate((0.2, 0.7))
+    ]
+    aggregate = aggregate_observations(observations)
+    assert (
+        aggregate["tag_quality"]["t0"][
+            "maximum_reprojection_error_px"
+        ]
+        == 0.7
+    )
+
+
 def test_state_is_interpolated_at_camera_timestamp() -> None:
     before = tuple([255.0] + [0.0] * 19)
     after = tuple([235.0] + [0.0] * 19)

src/g20_thumb_apriltag_calibration/test/test_config.py

@@ -18,19 +18,57 @@ def test_front_tag_parameters_match_namespaced_detector() -> None:
     assert detector["tag"]["frames"] == calibration["tag_frames"]
     assert detector["tag"]["sizes"] == calibration["tag_sizes_m"]
     assert detector["tag"]["ids"] == [0, 1, 2, 3]
+    assert detector["qos_profile"] == "sensor_data"
     assert detector["detector"]["decimate"] == 1.5
     assert detector["detector"]["refine"] is True
     assert detector["detector"]["debug"] is False
 
 
-def test_trial_uses_image_plane_angles_and_thirty_pixel_tags() -> None:
+def test_trial_uses_centre_trajectory_and_thirty_pixel_tags() -> None:
     config = yaml.safe_load(
         (PACKAGE_ROOT / "config" / "calibration.yaml").read_text()
     )
     parameters = config["g20_thumb_calibration"]["ros__parameters"]
 
-    assert parameters["angle_estimation_mode"] == "image_plane_2d"
+    assert parameters["angle_estimation_mode"] == "trajectory_center_3d"
+    assert parameters["passive_ip_multiplier"] == 1.02
+    assert parameters["pnp_minimum_valid_rate"] == 0.95
+    assert parameters["pnp_maximum_reprojection_error_px"] <= 1.5
+    assert parameters["pnp_reprojection_tie_px"] == 1.5
+    assert parameters["pnp_tracker_reset_seconds"] == 5.0
+    assert parameters["pnp_group_relative_rotation_scale_deg"] == 5.0
+    assert parameters["pnp_group_relative_translation_scale_m"] == 0.01
+    assert parameters["pnp_group_reprojection_weight"] == 0.05
+    assert parameters["pnp_trajectory_reprojection_scale_px"] == 0.1
+    assert parameters["pnp_rigid_rotation_scale_deg"] == 5.0
+    assert parameters["pnp_rigid_p95_accepted_drift_deg"] == 8.0
+    assert parameters["pnp_rigid_maximum_accepted_drift_deg"] == 15.0
+    assert (
+        parameters["pnp_rigid_p95_accepted_distance_drift_m"]
+        <= 0.003
+    )
+    assert (
+        parameters["pnp_rigid_maximum_accepted_distance_drift_m"]
+        <= 0.006
+    )
+    assert parameters["pnp_maximum_pose_jump_deg"] <= 35.0
+    assert parameters["trajectory_maximum_plane_rms_m"] <= 0.004
+    assert parameters["trajectory_maximum_radial_rms_m"] <= 0.004
+    assert parameters["trajectory_minimum_radius_m"] >= 0.005
+    assert parameters["trajectory_minimum_arc_deg"] >= 15.0
+    assert (
+        parameters["trajectory_maximum_root_role_disagreement_deg"]
+        <= 5.0
+    )
+    assert parameters["trajectory_maximum_anchor_drift_m"] <= 0.005
+    assert parameters["trajectory_static_translation_outlier_m"] <= 0.005
+    assert (
+        parameters["trajectory_maximum_static_translation_rms_m"]
+        <= 0.002
+    )
     assert parameters["minimum_edge_pixels"] == 30.0
+    assert parameters["maximum_static_std_deg"] == 3.0
+    assert parameters["minimum_pose_inlier_rate"] == 0.90
     assert parameters["repetitions"] == 1
     assert parameters["command_step"] == 8
     assert parameters["scan_mode"] == "continuous"
@@ -39,6 +77,8 @@ def test_trial_uses_image_plane_angles_and_thirty_pixel_tags() -> None:
     assert parameters["minimum_detection_hz"] == 15.0
     assert parameters["stable_frames"] == 5
     assert parameters["capture_frames"] == 8
+    assert parameters["maximum_stable_spread_deg"] == 3.0
+    assert parameters["maximum_stable_translation_spread_m"] <= 0.003
     assert parameters["validation_command_count"] == 5
     assert parameters["continuous_minimum_bins"] >= 32
     assert parameters["continuous_maximum_bin_gap"] <= 16

src/g20_thumb_apriltag_calibration/test/test_diagnostics.py

@@ -0,0 +1,105 @@
+from g20_thumb_apriltag_calibration.acquisition import TagQuality
+from g20_thumb_apriltag_calibration.diagnostics import (
+    build_tag_quality_diagnostics,
+    render_status_text_zh,
+    status_guidance_zh,
+)
+
+
+def _tag_config() -> dict[str, dict]:
+    return {
+        role: {"id": tag_id, "frame": role, "size_m": 0.01}
+        for role, tag_id in zip(("t0", "t3", "t4", "t5"), range(4))
+    }
+
+
+def test_invalid_t0_edge_has_specific_chinese_guidance() -> None:
+    qualities = {
+        role: TagQuality(
+            hamming=0,
+            decision_margin=100.0,
+            edge_pixels=29.4 if role == "t0" else 34.0,
+        )
+        for role in _tag_config()
+    }
+    diagnostics = build_tag_quality_diagnostics(
+        _tag_config(),
+        qualities,
+        {"t0": "tag_quality_invalid"},
+        {},
+        maximum_hamming=0,
+        minimum_decision_margin=30.0,
+        minimum_edge_pixels=30.0,
+        maximum_reprojection_error_px=1.5,
+    )
+
+    assert diagnostics["t0"]["individual_valid"] is False
+    assert diagnostics["t0"]["edge_pixels"] == 29.4
+    assert "边长29.4px" in diagnostics["t0"]["summary_zh"]
+    assert diagnostics["t3"]["individual_valid"] is True
+
+    reason_zh, action_zh = status_guidance_zh(
+        "PAUSED",
+        "point_capture_failed:invalid_tag_frame",
+        diagnostics,
+    )
+    assert "掌心T0" in reason_zh
+    assert "边长29.4px" in reason_zh
+    assert "相机稍微靠近" in action_zh
+    assert "resume" in action_zh
+    text = render_status_text_zh(
+        "标定已暂停",
+        reason_zh,
+        action_zh,
+        diagnostics,
+    )
+    assert "状态：标定已暂停" in text
+    assert "掌心T0(ID 0)：异常，边长29.4px" in text
+    assert "拇指根部T3(ID 1)：正常" in text
+
+
+def test_missing_tag_is_reported_without_manual_topic_parsing() -> None:
+    diagnostics = build_tag_quality_diagnostics(
+        _tag_config(),
+        {},
+        {"t5": "tag_not_detected"},
+        {},
+        maximum_hamming=0,
+        minimum_decision_margin=30.0,
+        minimum_edge_pixels=30.0,
+        maximum_reprojection_error_px=1.5,
+    )
+
+    assert diagnostics["t5"]["detected"] is False
+    assert "未检测到" in diagnostics["t5"]["summary_zh"]
+    reason_zh, action_zh = status_guidance_zh(
+        "PAUSED",
+        "point_capture_failed:invalid_tag_frame",
+        diagnostics,
+    )
+    assert "拇指末节T5" in reason_zh
+    assert "四张标签同时可见" in action_zh
+
+
+def test_low_detection_frequency_has_direct_chinese_action() -> None:
+    reason_zh, action_zh = status_guidance_zh(
+        "PREFLIGHT",
+        "detection_hz_too_low:11.29",
+        {},
+    )
+
+    assert "11.29Hz" in reason_zh
+    assert "额外订阅" in action_zh
+
+
+def test_synchronised_timeout_explains_resume_not_start() -> None:
+    reason_zh, action_zh = status_guidance_zh(
+        "PAUSED",
+        "continuous_sweep_failed:synchronised_tag_state_timeout",
+        {},
+    )
+
+    assert "运动过程中连续3秒" in reason_zh
+    assert "当前画面恢复正常" in reason_zh
+    assert "resume" in action_zh
+    assert "不要调用start" in action_zh

src/g20_thumb_apriltag_calibration/test/test_pnp.py

@@ -0,0 +1,542 @@
+from __future__ import annotations
+
+import cv2
+import numpy as np
+import pytest
+from scipy.spatial.transform import Rotation
+
+from g20_thumb_apriltag_calibration.pnp import (
+    SquareTagGroupPoseTracker,
+    SquareTagPose,
+    SquareTagPoseTracker,
+    rotation_distance_rad,
+    select_continuous_pose,
+    select_rigid_group_trajectory,
+    solve_square_tag_ippe,
+    square_object_points,
+)
+
+
+def _camera_matrix() -> np.ndarray:
+    return np.asarray(
+        [
+            [650.0, 0.0, 640.0],
+            [0.0, 650.0, 360.0],
+            [0.0, 0.0, 1.0],
+        ]
+    )
+
+
+def _project(
+    rotation: Rotation,
+    translation_xyz_m: np.ndarray,
+    *,
+    tag_size_m: float = 0.01,
+) -> np.ndarray:
+    rotation_vector, _ = cv2.Rodrigues(rotation.as_matrix())
+    corners, _ = cv2.projectPoints(
+        square_object_points(tag_size_m),
+        rotation_vector,
+        translation_xyz_m,
+        _camera_matrix(),
+        np.zeros((4, 1)),
+    )
+    return corners.reshape(4, 2)
+
+
+def test_ippe_recovers_known_square_tag_pose() -> None:
+    expected_rotation = Rotation.from_euler(
+        "xyz", [10.0, -15.0, 25.0], degrees=True
+    )
+    expected_translation = np.asarray([0.02, -0.01, 0.25])
+    candidates = solve_square_tag_ippe(
+        _project(expected_rotation, expected_translation),
+        tag_size_m=0.01,
+        camera_matrix=_camera_matrix(),
+    )
+
+    assert len(candidates) == 2
+    actual = min(candidates, key=lambda item: item.reprojection_error_px)
+    assert (
+        rotation_distance_rad(
+            expected_rotation.as_quat(),
+            actual.quaternion_xyzw,
+        )
+        < 1.0e-8
+    )
+    assert np.allclose(actual.translation_xyz_m, expected_translation)
+    assert actual.reprojection_error_px < 1.0e-8
+
+
+def test_temporal_selection_breaks_near_reprojection_tie() -> None:
+    previous = SquareTagPose(
+        quaternion_xyzw=(0.0, 0.0, 0.0, 1.0),
+        translation_xyz_m=(0.0, 0.0, 0.25),
+        reprojection_error_px=0.2,
+    )
+    continuous = SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler("y", 2.0, degrees=True).as_quat()
+        ),
+        translation_xyz_m=(0.001, 0.0, 0.25),
+        reprojection_error_px=0.11,
+    )
+    flipped = SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler("y", 55.0, degrees=True).as_quat()
+        ),
+        translation_xyz_m=(0.0, 0.0, 0.25),
+        reprojection_error_px=0.1,
+    )
+
+    selected, reason = select_continuous_pose(
+        [flipped, continuous],
+        previous=previous,
+        maximum_reprojection_error_px=1.5,
+        reprojection_tie_px=0.03,
+        maximum_pose_jump_rad=np.deg2rad(35.0),
+        maximum_translation_jump_m=0.04,
+        maximum_tag_tilt_rad=np.deg2rad(75.0),
+    )
+
+    assert reason == ""
+    assert selected == continuous
+
+
+def test_clear_reprojection_advantage_releases_stale_mirror_branch() -> None:
+    stale_mirror = SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler("y", 55.0, degrees=True).as_quat()
+        ),
+        translation_xyz_m=(0.0, 0.0, 0.25),
+        reprojection_error_px=0.25,
+    )
+    true_pose = SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler("y", 2.0, degrees=True).as_quat()
+        ),
+        translation_xyz_m=(0.001, 0.0, 0.25),
+        reprojection_error_px=0.05,
+    )
+    continued_mirror = SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler("y", 54.0, degrees=True).as_quat()
+        ),
+        translation_xyz_m=(0.0, 0.0, 0.25),
+        reprojection_error_px=0.24,
+    )
+
+    selected, reason = select_continuous_pose(
+        [continued_mirror, true_pose],
+        previous=stale_mirror,
+        maximum_reprojection_error_px=1.5,
+        reprojection_tie_px=0.03,
+        maximum_pose_jump_rad=np.deg2rad(35.0),
+        maximum_translation_jump_m=0.04,
+        maximum_tag_tilt_rad=np.deg2rad(75.0),
+    )
+
+    assert reason == ""
+    assert selected == true_pose
+
+
+def test_active_motion_can_prioritise_continuous_branch() -> None:
+    previous = SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler("y", 55.0, degrees=True).as_quat()
+        ),
+        translation_xyz_m=(0.0, 0.0, 0.25),
+        reprojection_error_px=0.25,
+    )
+    continuous = SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler("y", 54.0, degrees=True).as_quat()
+        ),
+        translation_xyz_m=(0.0, 0.0, 0.25),
+        reprojection_error_px=0.24,
+    )
+    discontinuous = SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler("y", 2.0, degrees=True).as_quat()
+        ),
+        translation_xyz_m=(0.001, 0.0, 0.25),
+        reprojection_error_px=0.05,
+    )
+
+    selected, reason = select_continuous_pose(
+        [continuous, discontinuous],
+        previous=previous,
+        maximum_reprojection_error_px=1.5,
+        reprojection_tie_px=1.5,
+        maximum_pose_jump_rad=np.deg2rad(35.0),
+        maximum_translation_jump_m=0.04,
+        maximum_tag_tilt_rad=np.deg2rad(75.0),
+    )
+
+    assert reason == ""
+    assert selected == continuous
+
+
+def test_tracker_recovers_after_timestamp_gap() -> None:
+    tracker = SquareTagPoseTracker(
+        maximum_reprojection_error_px=1.5,
+        reprojection_tie_px=1.5,
+        maximum_pose_jump_rad=np.deg2rad(5.0),
+        maximum_translation_jump_m=0.04,
+        maximum_tag_tilt_rad=np.deg2rad(75.0),
+        reset_after_seconds=0.5,
+    )
+    first_rotation = Rotation.from_euler("y", 0.0, degrees=True)
+    second_rotation = Rotation.from_euler("y", 20.0, degrees=True)
+    first, first_reason = tracker.estimate(
+        "t0",
+        _project(first_rotation, np.asarray([0.0, 0.0, 0.25])),
+        tag_size_m=0.01,
+        camera_matrix=_camera_matrix(),
+        stamp_ns=1_000_000_000,
+    )
+    rejected, rejection_reason = tracker.estimate(
+        "t0",
+        _project(second_rotation, np.asarray([0.0, 0.0, 0.25])),
+        tag_size_m=0.01,
+        camera_matrix=_camera_matrix(),
+        stamp_ns=1_100_000_000,
+    )
+    recovered, recovered_reason = tracker.estimate(
+        "t0",
+        _project(second_rotation, np.asarray([0.0, 0.0, 0.25])),
+        tag_size_m=0.01,
+        camera_matrix=_camera_matrix(),
+        stamp_ns=1_700_000_000,
+    )
+
+    assert first is not None
+    assert first_reason == ""
+    assert rejected is None
+    assert rejection_reason == "pose_jump"
+    assert recovered is not None
+    assert recovered_reason == ""
+
+
+def _pose(
+    rotation_deg: float,
+    x_m: float,
+    reprojection_error_px: float,
+) -> SquareTagPose:
+    return SquareTagPose(
+        quaternion_xyzw=tuple(
+            Rotation.from_euler(
+                "y", rotation_deg, degrees=True
+            ).as_quat()
+        ),
+        translation_xyz_m=(x_m, 0.0, 0.25),
+        reprojection_error_px=reprojection_error_px,
+    )
+
+
+def test_group_tracker_prevents_incompatible_t4_t5_branch_switch() -> None:
+    tracker = SquareTagGroupPoseTracker(
+        roles=("t0", "t3", "t4", "t5"),
+        adjacent_pairs=(
+            ("t0", "t3"),
+            ("t3", "t4"),
+            ("t4", "t5"),
+        ),
+        maximum_pose_jump_rad=np.deg2rad(35.0),
+        maximum_translation_jump_m=0.04,
+        relative_rotation_scale_rad=np.deg2rad(5.0),
+        relative_translation_scale_m=0.01,
+        reprojection_scale_px=0.1,
+        reprojection_weight=0.05,
+        reset_after_seconds=5.0,
+    )
+    first = {
+        "t0": (_pose(0.0, 0.00, 0.05),),
+        "t3": (_pose(5.0, 0.03, 0.05),),
+        "t4": (_pose(15.0, 0.06, 0.05),),
+        "t5": (_pose(25.0, 0.09, 0.05),),
+    }
+    selected_first, first_reason = tracker.select(
+        first,
+        stamp_ns=1_000_000_000,
+    )
+    assert first_reason == ""
+    assert selected_first is not None
+
+    # The per-tag minimum-error solutions move only a few degrees and can
+    # therefore fool independent trackers. Together they change T4->T5 by
+    # 8 deg; the slightly higher-error pair preserves the physical chain.
+    continuous_t4 = _pose(16.0, 0.061, 0.20)
+    continuous_t5 = _pose(26.0, 0.091, 0.20)
+    independent_best_t4 = _pose(19.0, 0.061, 0.05)
+    independent_best_t5 = _pose(21.0, 0.091, 0.05)
+    second = {
+        "t0": (_pose(0.2, 0.00, 0.05),),
+        "t3": (_pose(5.2, 0.03, 0.05),),
+        "t4": (independent_best_t4, continuous_t4),
+        "t5": (independent_best_t5, continuous_t5),
+    }
+    selected, reason = tracker.select(
+        second,
+        stamp_ns=1_033_000_000,
+    )
+
+    assert reason == ""
+    assert selected is not None
+    assert selected["t4"] == continuous_t4
+    assert selected["t5"] == continuous_t5
+    assert tracker.branch_correction_counts == {"t4": 1, "t5": 1}
+
+
+def test_group_tracker_keeps_same_pair_across_sweep_turnaround() -> None:
+    tracker = SquareTagGroupPoseTracker(
+        roles=("t4", "t5"),
+        adjacent_pairs=(("t4", "t5"),),
+        maximum_pose_jump_rad=np.deg2rad(35.0),
+        maximum_translation_jump_m=0.04,
+        relative_rotation_scale_rad=np.deg2rad(5.0),
+        relative_translation_scale_m=0.01,
+        reprojection_scale_px=0.1,
+        reprojection_weight=0.05,
+        reset_after_seconds=5.0,
+    )
+    true_t4 = _pose(30.0, 0.06, 0.05)
+    true_t5 = _pose(65.0, 0.09, 0.05)
+    selected, _ = tracker.select(
+        {"t4": (true_t4,), "t5": (true_t5,)},
+        stamp_ns=1_000_000_000,
+    )
+    assert selected is not None
+
+    return_t4 = _pose(29.5, 0.06, 0.20)
+    return_t5 = _pose(64.5, 0.09, 0.20)
+    mirror_t4 = _pose(33.0, 0.06, 0.04)
+    mirror_t5 = _pose(57.0, 0.09, 0.04)
+    selected, reason = tracker.select(
+        {
+            "t4": (mirror_t4, return_t4),
+            "t5": (mirror_t5, return_t5),
+        },
+        stamp_ns=1_033_000_000,
+    )
+
+    assert reason == ""
+    assert selected == {"t4": return_t4, "t5": return_t5}
+
+
+def test_whole_trajectory_recovers_rigid_group_from_mirror_drift() -> None:
+    roles = ("t3", "t4", "t5")
+    mount_rotations = {
+        "t3": Rotation.identity(),
+        "t4": Rotation.from_euler("z", 20.0, degrees=True),
+        "t5": Rotation.from_euler("z", -15.0, degrees=True),
+    }
+    mount_positions = {
+        "t3": np.asarray([0.0, 0.0, 0.0]),
+        "t4": np.asarray([0.025, 0.0, 0.0]),
+        "t5": np.asarray([0.05, 0.0, 0.0]),
+    }
+    false_factors = {"t3": 0.5, "t4": -0.5, "t5": 1.0}
+    frames = []
+    true_frames = []
+    for angle_deg in np.linspace(0.0, 45.0, 30):
+        group_rotation = Rotation.from_euler(
+            "y", angle_deg, degrees=True
+        )
+        origin = np.asarray([0.0, 0.0, 0.3])
+        candidates = {}
+        truths = {}
+        for role in roles:
+            true_rotation = group_rotation * mount_rotations[role]
+            true_translation = origin + group_rotation.apply(
+                mount_positions[role]
+            )
+            true_pose = SquareTagPose(
+                quaternion_xyzw=tuple(true_rotation.as_quat()),
+                translation_xyz_m=tuple(true_translation),
+                reprojection_error_px=0.10,
+            )
+            false_rotation = true_rotation * Rotation.from_euler(
+                "x",
+                false_factors[role] * angle_deg,
+                degrees=True,
+            )
+            false_pose = SquareTagPose(
+                quaternion_xyzw=tuple(false_rotation.as_quat()),
+                translation_xyz_m=tuple(
+                    true_translation
+                    + np.asarray(
+                        [
+                            0.0,
+                            false_factors[role] * angle_deg / 10000.0,
+                            0.0,
+                        ]
+                    )
+                ),
+                reprojection_error_px=0.05,
+            )
+            candidates[role] = (false_pose, true_pose)
+            truths[role] = true_pose
+        frames.append(candidates)
+        true_frames.append(truths)
+
+    selected, quality = select_rigid_group_trajectory(
+        frames,
+        roles=roles,
+        fixed_pairs=(("t3", "t4"), ("t4", "t5")),
+        reprojection_scale_px=0.1,
+        rotation_scale_rad=np.deg2rad(5.0),
+        translation_scale_m=0.01,
+    )
+
+    for role in roles:
+        assert (
+            rotation_distance_rad(
+                selected[-1][role].quaternion_xyzw,
+                true_frames[-1][role].quaternion_xyzw,
+            )
+            < 1.0e-8
+        )
+    assert quality["maximum_pair_rotation_drift_rad"] < np.deg2rad(5.0)
+    assert quality["p95_pair_rotation_drift_rad"] < np.deg2rad(5.0)
+
+
+def test_trajectory_quality_uses_robust_rigid_reference() -> None:
+    frames = []
+    for index in range(30):
+        child_rotation = Rotation.identity()
+        if index == 0:
+            child_rotation = Rotation.from_euler(
+                "x", 10.0, degrees=True
+            )
+        frames.append(
+            {
+                "parent": (
+                    SquareTagPose(
+                        quaternion_xyzw=tuple(
+                            Rotation.identity().as_quat()
+                        ),
+                        translation_xyz_m=(0.0, 0.0, 0.3),
+                        reprojection_error_px=0.1,
+                    ),
+                ),
+                "child": (
+                    SquareTagPose(
+                        quaternion_xyzw=tuple(child_rotation.as_quat()),
+                        translation_xyz_m=(0.03, 0.0, 0.3),
+                        reprojection_error_px=0.1,
+                    ),
+                ),
+            }
+        )
+
+    _, quality = select_rigid_group_trajectory(
+        frames,
+        roles=("parent", "child"),
+        fixed_pairs=(("parent", "child"),),
+        reprojection_scale_px=0.1,
+        rotation_scale_rad=np.deg2rad(5.0),
+        translation_scale_m=0.01,
+    )
+
+    assert quality["maximum_pair_rotation_drift_rad"] == pytest.approx(
+        np.deg2rad(10.0)
+    )
+    assert quality["p95_pair_rotation_drift_rad"] == pytest.approx(0.0)
+    assert quality["median_pair_rotation_drift_rad"] == pytest.approx(0.0)
+
+
+def test_distance_geometry_ignores_planar_orientation_drift() -> None:
+    frames = []
+    for angle_deg in np.linspace(0.0, 45.0, 30):
+        group = Rotation.from_euler("y", angle_deg, degrees=True)
+        origin = np.asarray([0.0, 0.0, 0.3])
+        parent_position = origin
+        child_position = origin + group.apply([0.04, 0.0, 0.0])
+        # The centres form a perfect rigid pair, while the planar-PnP parent
+        # orientation contains a pose-dependent error.
+        parent_rotation = group * Rotation.from_euler(
+            "z", 0.25 * angle_deg, degrees=True
+        )
+        frames.append(
+            {
+                "parent": (
+                    SquareTagPose(
+                        quaternion_xyzw=tuple(parent_rotation.as_quat()),
+                        translation_xyz_m=tuple(parent_position),
+                        reprojection_error_px=0.1,
+                    ),
+                ),
+                "child": (
+                    SquareTagPose(
+                        quaternion_xyzw=tuple(group.as_quat()),
+                        translation_xyz_m=tuple(child_position),
+                        reprojection_error_px=0.1,
+                    ),
+                ),
+            }
+        )
+
+    _, quality = select_rigid_group_trajectory(
+        frames,
+        roles=("parent", "child"),
+        fixed_pairs=(("parent", "child"),),
+        reprojection_scale_px=0.1,
+        rotation_scale_rad=np.deg2rad(5.0),
+        translation_scale_m=0.01,
+        pair_geometry="distance",
+    )
+
+    assert quality["pair_geometry"] == "distance"
+    assert quality["maximum_pair_distance_drift_m"] < 1.0e-10
+    assert quality["p95_pair_distance_drift_m"] < 1.0e-10
+    assert quality["p95_pair_translation_drift_m"] > 0.001
+
+
+def test_distance_geometry_rejects_pose_branch_with_changing_length() -> None:
+    frames = []
+    true_children = []
+    for index in range(30):
+        parent = SquareTagPose(
+            quaternion_xyzw=tuple(Rotation.identity().as_quat()),
+            translation_xyz_m=(0.0, 0.0, 0.3),
+            reprojection_error_px=0.1,
+        )
+        true_child = SquareTagPose(
+            quaternion_xyzw=tuple(Rotation.identity().as_quat()),
+            translation_xyz_m=(0.04, 0.0, 0.3),
+            reprojection_error_px=0.1,
+        )
+        false_child = SquareTagPose(
+            quaternion_xyzw=tuple(
+                Rotation.from_euler("x", 10.0, degrees=True).as_quat()
+            ),
+            translation_xyz_m=(
+                0.04,
+                0.020 * np.sin(np.pi * index / 29.0),
+                0.3,
+            ),
+            reprojection_error_px=0.05,
+        )
+        frames.append(
+            {
+                "parent": (parent,),
+                "child": (false_child, true_child),
+            }
+        )
+        true_children.append(true_child)
+
+    selected, quality = select_rigid_group_trajectory(
+        frames,
+        roles=("parent", "child"),
+        fixed_pairs=(("parent", "child"),),
+        reprojection_scale_px=0.1,
+        rotation_scale_rad=np.deg2rad(5.0),
+        translation_scale_m=0.002,
+        pair_geometry="distance",
+    )
+
+    assert selected[len(selected) // 2]["child"] == (
+        true_children[len(true_children) // 2]
+    )
+    assert quality["p95_pair_distance_drift_m"] < 1.0e-6

src/g20_thumb_apriltag_calibration/test/test_trajectory.py

@@ -0,0 +1,295 @@
+from __future__ import annotations
+
+import numpy as np
+import pytest
+from scipy.spatial.transform import Rotation
+
+from g20_thumb_apriltag_calibration.core import (
+    DIRECTION_DECREASING,
+    DIRECTION_INCREASING,
+    PAIR_IP,
+    PAIR_MCP,
+    PAIR_ROOT,
+    PHASE_ROOT,
+    PHASE_TIP,
+    create_final_payload,
+)
+from g20_thumb_apriltag_calibration.trajectory import (
+    _regularize_coupled_zero_tail,
+    fit_center_trajectory_curves,
+    maximum_center_non_target_drift_rad,
+    measure_center_trajectory_angles,
+)
+
+
+def _quat(angle: float) -> list[float]:
+    return [
+        float(value)
+        for value in Rotation.from_rotvec([0.0, 0.0, angle]).as_quat()
+    ]
+
+
+def _records(
+    *,
+    camera_rotation: Rotation = Rotation.identity(),
+    camera_translation: np.ndarray = np.zeros(3),
+    tag_shift: float = 0.0,
+) -> list[dict]:
+    commands = list(range(0, 256, 8))
+    if commands[-1] != 255:
+        commands.append(255)
+    directions = (
+        (DIRECTION_DECREASING, list(reversed(commands))),
+        (DIRECTION_INCREASING, commands),
+    )
+    t0 = np.asarray([0.0, 0.0, 0.55])
+    root_centre = np.asarray([0.025, -0.010, 0.55])
+    root_points = {
+        "t3": np.asarray([0.060 + tag_shift, -0.005, 0.55]),
+        "t4": np.asarray([0.090, 0.002 + tag_shift, 0.55]),
+        "t5": np.asarray([0.120, 0.009, 0.55 + tag_shift]),
+    }
+    mcp_centre = np.asarray([0.055, -0.004, 0.0])
+    t3_tip = np.asarray([0.0, 0.0, 0.55])
+    t4_reference = t3_tip + np.asarray([0.080, 0.006 + tag_shift, 0.0])
+    ip_centre = t3_tip + np.asarray([0.095, 0.006, 0.0])
+    t5_reference = t3_tip + np.asarray([0.125, 0.008 + tag_shift, 0.0])
+
+    def camera(point: np.ndarray) -> np.ndarray:
+        return camera_rotation.apply(point) + camera_translation
+
+    records: list[dict] = []
+    for phase in (PHASE_ROOT, PHASE_TIP):
+        for direction, ordered_commands in directions:
+            for command in ordered_commands:
+                progress = (255.0 - command) / 255.0
+                root_angle = 0.80 * progress if phase == PHASE_ROOT else 0.0
+                mcp_angle = 1.15 * progress if phase == PHASE_TIP else 0.0
+                ip_angle = 1.02 * mcp_angle if phase == PHASE_TIP else 0.0
+                if phase == PHASE_ROOT:
+                    root_rotation = Rotation.from_rotvec(
+                        [0.0, 0.0, root_angle]
+                    )
+                    positions = {
+                        "t0": t0,
+                        **{
+                            role: root_centre
+                            + root_rotation.apply(point - root_centre)
+                            for role, point in root_points.items()
+                        },
+                    }
+                else:
+                    mcp_rotation = Rotation.from_rotvec(
+                        [0.0, 0.0, mcp_angle]
+                    )
+                    ip_rotation = Rotation.from_rotvec(
+                        [0.0, 0.0, ip_angle]
+                    )
+                    # The MCP centre below is expressed relative to T3.
+                    mcp_world = t3_tip + mcp_centre
+                    t4 = mcp_world + mcp_rotation.apply(
+                        t4_reference - mcp_world
+                    )
+                    ip_at_zero = ip_centre
+                    t5_inside_parent = ip_at_zero + ip_rotation.apply(
+                        t5_reference - ip_at_zero
+                    )
+                    t5 = mcp_world + mcp_rotation.apply(
+                        t5_inside_parent - mcp_world
+                    )
+                    positions = {
+                        "t0": t0,
+                        "t3": t3_tip,
+                        "t4": t4,
+                        "t5": t5,
+                    }
+                records.append(
+                    {
+                        "kind": "sample",
+                        "phase": phase,
+                        "cycle": 0,
+                        "direction": direction,
+                        "command_u8": command,
+                        "relative_quaternion_xyzw": {
+                            PAIR_ROOT: _quat(root_angle),
+                            PAIR_MCP: _quat(mcp_angle),
+                            PAIR_IP: _quat(ip_angle),
+                        },
+                        "tag_translation_xyz_m": {
+                            role: [
+                                float(value) for value in camera(point)
+                            ]
+                            for role, point in positions.items()
+                        },
+                    }
+                )
+    return records
+
+
+def test_centre_trajectory_recovers_three_joint_angles_and_zero() -> None:
+    records = _records()
+    fit = fit_center_trajectory_curves(
+        records,
+        maximum_plane_rms_m=0.001,
+        maximum_radial_rms_m=0.001,
+        maximum_anchor_drift_m=0.001,
+    )
+    assert fit.measurement_mode == "trajectory_center_3d"
+    assert fit.joints["thumb_cmc_pitch"]["angle_rad"][0] == pytest.approx(
+        0.80, abs=2.0e-3
+    )
+    assert fit.joints["thumb_mcp"]["angle_rad"][0] == pytest.approx(
+        1.15, abs=2.0e-3
+    )
+    assert fit.joints["thumb_ip"]["angle_rad"][0] == pytest.approx(
+        1.173, abs=3.0e-3
+    )
+    for joint in fit.joints.values():
+        assert joint["angle_rad"][255] == 0.0
+    payload = create_final_payload(
+        serial_number="G20_LEFT_TRAJECTORY_TEST",
+        fit=fit,
+        validation_errors_rad=[0.01, -0.01],
+        passed=True,
+    )
+    assert payload["zero_command_u8"] == 255
+    for joint in payload["joints"].values():
+        assert len(joint["angle_rad"]) == 256
+        assert joint["angle_rad"][255] == 0.0
+
+
+def test_centre_trajectory_is_invariant_to_camera_and_tag_offset() -> None:
+    reference = fit_center_trajectory_curves(_records())
+    changed = fit_center_trajectory_curves(
+        _records(
+            camera_rotation=Rotation.from_euler(
+                "xyz", [0.35, -0.25, 0.20]
+            ),
+            camera_translation=np.asarray([0.12, -0.04, 0.08]),
+            tag_shift=0.004,
+        )
+    )
+    for joint_name in ("thumb_cmc_pitch", "thumb_mcp", "thumb_ip"):
+        assert changed.joints[joint_name]["angle_rad"] == pytest.approx(
+            reference.joints[joint_name]["angle_rad"],
+            abs=6.0e-3,
+        )
+
+
+def test_passive_ip_uses_mimic_constraint_despite_distal_pnp_bias() -> None:
+    reference = fit_center_trajectory_curves(_records())
+    biased_records = _records(
+        camera_rotation=Rotation.from_euler(
+            "xyz", [-0.28, 0.31, -0.16]
+        ),
+        camera_translation=np.asarray([-0.08, 0.03, 0.11]),
+    )
+    for record in biased_records:
+        if record["phase"] != PHASE_TIP:
+            continue
+        progress = (255.0 - float(record["command_u8"])) / 255.0
+        bias = np.asarray(
+            [
+                0.0012 * np.sin(1.7 * progress),
+                0.0008 * progress * progress,
+                -0.0006 * np.sin(2.3 * progress),
+            ]
+        )
+        record["tag_translation_xyz_m"]["t5"] = [
+            float(value)
+            for value in (
+                np.asarray(
+                    record["tag_translation_xyz_m"]["t5"], dtype=float
+                )
+                + bias
+            )
+        ]
+
+    biased = fit_center_trajectory_curves(biased_records)
+    for fit in (reference, biased):
+        mcp = np.asarray(fit.joints["thumb_mcp"]["angle_rad"])
+        ip = np.asarray(fit.joints["thumb_ip"]["angle_rad"])
+        assert ip == pytest.approx(1.02 * mcp, abs=1.1e-8)
+        assert fit.ip_coupling["multiplier"] == pytest.approx(1.02)
+        assert fit.ip_coupling["offset_rad"] == 0.0
+        assert fit.ip_coupling["constrained_r_squared"] == 1.0
+        assert fit.ip_coupling["r_squared"] > 0.98
+
+    assert biased.joints["thumb_ip"]["angle_rad"] == pytest.approx(
+        reference.joints["thumb_ip"]["angle_rad"],
+        abs=6.0e-3,
+    )
+    assert (
+        biased.trajectory_quality["tip"][
+            "ip_observed_vs_constrained_max_rad"
+        ]
+        > 0.0
+    )
+
+
+def test_passive_ip_multiplier_is_configurable() -> None:
+    fit = fit_center_trajectory_curves(
+        _records(),
+        passive_ip_multiplier=0.97,
+    )
+    mcp = np.asarray(fit.joints["thumb_mcp"]["angle_rad"])
+    ip = np.asarray(fit.joints["thumb_ip"]["angle_rad"])
+    assert ip == pytest.approx(0.97 * mcp, abs=1.1e-8)
+
+
+def test_static_measurement_uses_fitted_serial_tip_model() -> None:
+    records = _records()
+    fit = fit_center_trajectory_curves(records)
+    command = 128
+    record = next(
+        item
+        for item in records
+        if item["phase"] == PHASE_TIP
+        and item["direction"] == DIRECTION_DECREASING
+        and item["command_u8"] == command
+    )
+    measured = measure_center_trajectory_angles(
+        fit.trajectory_models,
+        record["tag_translation_xyz_m"],
+    )
+    progress = (255.0 - command) / 255.0
+    assert measured["thumb_mcp"] == pytest.approx(
+        1.15 * progress, abs=2.0e-3
+    )
+    assert measured["thumb_ip"] == pytest.approx(
+        1.02 * 1.15 * progress, abs=3.0e-3
+    )
+
+
+def test_non_target_drift_is_measured_without_tag_orientations() -> None:
+    records = _records()
+    fit = fit_center_trajectory_curves(records)
+    assert maximum_center_non_target_drift_rad(
+        records, fit.trajectory_models
+    ) == pytest.approx(0.0, abs=3.0e-3)
+
+
+def test_short_ip_zero_tail_uses_coupled_mcp_shape() -> None:
+    mcp = np.linspace(1.0, 0.0, 256)
+    ip = 0.6 * mcp
+    ip[248:] = 0.0
+
+    regularized = _regularize_coupled_zero_tail(ip, mcp)
+
+    assert regularized[:248] == pytest.approx(ip[:248])
+    assert np.all(regularized[248:255] > 0.0)
+    assert np.all(np.diff(regularized) <= 1.0e-12)
+    assert regularized[255] == 0.0
+    assert regularized[248:255] == pytest.approx(
+        0.6 * mcp[248:255]
+    )
+
+
+def test_long_or_unresolved_ip_zero_tail_is_not_invented() -> None:
+    mcp = np.linspace(1.0, 0.0, 256)
+    ip = 0.6 * mcp
+    ip[220:] = 0.0
+
+    regularized = _regularize_coupled_zero_tail(ip, mcp)
+
+    assert regularized == pytest.approx(ip)