From 9210373fb2b68a6e0e5996b5cee2dfba5846f028 Mon Sep 17 00:00:00 2001 From: lxp <2770281812@qq.com> Date: Wed, 29 Jul 2026 18:30:08 +0800 Subject: [PATCH] =?UTF-8?q?=E5=9C=86=E5=BF=83=E6=A0=87=E5=AE=9A=E6=96=B9?= =?UTF-8?q?=E6=A1=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/g20_thumb_apriltag_calibration/README.md | 92 +- .../config/calibration.yaml | 75 +- .../config/front_tags.yaml | 4 + .../acquisition.py | 251 +++- .../g20_thumb_apriltag_calibration/core.py | 5 +- .../diagnostics.py | 287 ++++ .../g20_thumb_apriltag_calibration/node.py | 1181 ++++++++++++++++- .../g20_thumb_apriltag_calibration/pnp.py | 979 ++++++++++++++ .../trajectory.py | 1046 +++++++++++++++ .../launch/front_thumb_calibration.launch.py | 8 + .../test/test_acquisition.py | 184 +++ .../test/test_config.py | 44 +- .../test/test_diagnostics.py | 105 ++ .../test/test_pnp.py | 542 ++++++++ .../test/test_trajectory.py | 295 ++++ 15 files changed, 5013 insertions(+), 85 deletions(-) create mode 100644 src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/diagnostics.py create mode 100644 src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py create mode 100644 src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/trajectory.py create mode 100644 src/g20_thumb_apriltag_calibration/test/test_diagnostics.py create mode 100644 src/g20_thumb_apriltag_calibration/test/test_pnp.py create mode 100644 src/g20_thumb_apriltag_calibration/test/test_trajectory.py diff --git a/src/g20_thumb_apriltag_calibration/README.md b/src/g20_thumb_apriltag_calibration/README.md index 392a5f1..b4b7460 100644 --- a/src/g20_thumb_apriltag_calibration/README.md +++ b/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 分卷。快速标定通常不需要录制； diff --git a/src/g20_thumb_apriltag_calibration/config/calibration.yaml b/src/g20_thumb_apriltag_calibration/config/calibration.yaml index 68d095e..329747a 100644 --- a/src/g20_thumb_apriltag_calibration/config/calibration.yaml +++ b/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 diff --git a/src/g20_thumb_apriltag_calibration/config/front_tags.yaml b/src/g20_thumb_apriltag_calibration/config/front_tags.yaml index 0c385eb..a469078 100644 --- a/src/g20_thumb_apriltag_calibration/config/front_tags.yaml +++ b/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 diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/acquisition.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/acquisition.py index f423b04..d3eadeb 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/acquisition.py +++ b/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 diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/core.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/core.py index 75d7ce4..a8cd01a 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/core.py +++ b/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, diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/diagnostics.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/diagnostics.py new file mode 100644 index 0000000..8519170 --- /dev/null +++ b/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) diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/node.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/node.py index 0b4fde5..cda9daa 100644 --- a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/node.py +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/node.py @@ -3,6 +3,7 @@ from __future__ import annotations from collections import deque +from dataclasses import replace from datetime import datetime, timezone import hashlib import json @@ -23,13 +24,13 @@ from rclpy.qos import qos_profile_sensor_data from sensor_msgs.msg import CameraInfo, Image, JointState from std_msgs.msg import String from std_srvs.srv import Trigger -from tf2_msgs.msg import TFMessage from .acquisition import ( ContinuousSweepCollector, Observation, PointCollector, StateSample, + TAG_PAIR_ROLES, TagQuality, aggregate_sweep_observations, interpolate_state_u8, @@ -57,12 +58,31 @@ from .core import ( rotation_rms_rad, scan_targets, ) +from .diagnostics import ( + STATE_NAMES_ZH, + build_tag_quality_diagnostics, + pnp_rejection_zh, + render_status_text_zh, + status_guidance_zh, +) +from .pnp import ( + SquareTagGroupPoseTracker, + SquareTagPose, + SquareTagPoseTracker, + rotation_distance_rad, + select_rigid_group_trajectory, +) from .storage import ( append_jsonl, atomic_write_json, completed_scan_keys, load_jsonl, ) +from .trajectory import ( + fit_center_trajectory_curves, + maximum_center_non_target_drift_rad, + measure_center_trajectory_phase_angles, +) STATE_PREFLIGHT = "PREFLIGHT" @@ -114,13 +134,14 @@ class G20ThumbCalibrationNode(Node): self.pending_aggregate: dict[str, Any] | None = None self.pending_sweep_observations: list[Observation] | None = None self.fit: FitResult | None = None - self.validation_zero: dict[str, dict[str, list[float]]] = {} + self.validation_zero: dict[str, dict[str, Any]] = {} self.validation_errors: list[float] = [] self.latest_state_u8: tuple[float, ...] = () self.state_history: deque[StateSample] = deque(maxlen=600) self.latest_hand_info: dict[str, Any] = {} self.camera_intrinsics: dict[str, Any] = {} + self.pnp_camera_matrix: np.ndarray | None = None self.camera_info_valid = False self.camera_frame = "" self.preflight_detection_flags: deque[bool] = deque( @@ -129,6 +150,9 @@ class G20ThumbCalibrationNode(Node): self.preflight_detection_times: deque[float] = deque( maxlen=self.preflight_frames ) + self.preflight_pnp_flags: deque[bool] = deque( + maxlen=self.preflight_frames + ) self.preflight_observations: deque[Observation] = deque( maxlen=self.preflight_frames ) @@ -136,14 +160,68 @@ class G20ThumbCalibrationNode(Node): self.quality_cache: dict[int, dict[str, TagQuality]] = {} self.corner_cache: dict[int, dict[str, np.ndarray]] = {} self.transform_cache: dict[int, dict[str, tuple[float, ...]]] = {} + self.translation_cache: dict[ + int, dict[str, tuple[float, float, float]] + ] = {} + self.pnp_candidate_cache: dict[ + int, dict[str, tuple[SquareTagPose, ...]] + ] = {} self.emitted_stamps: set[int] = set() self.latest_corners: dict[str, np.ndarray] = {} + self.latest_tag_qualities: dict[str, TagQuality] = {} + self.latest_pnp_rejections: dict[str, str] = {} + self.latest_pnp_errors_px: dict[str, float] = {} + self.latest_trajectory_branch_quality: dict[str, Any] = {} + self.trajectory_branch_reports: list[dict[str, Any]] = [] + self.pnp_tracker = SquareTagPoseTracker( + maximum_reprojection_error_px=( + self.pnp_maximum_reprojection_error_px + ), + reprojection_tie_px=self.pnp_reprojection_tie_px, + maximum_pose_jump_rad=self.pnp_maximum_pose_jump_rad, + maximum_translation_jump_m=( + self.pnp_maximum_translation_jump_m + ), + maximum_tag_tilt_rad=self.pnp_maximum_tag_tilt_rad, + reset_after_seconds=self.pnp_tracker_reset_seconds, + ) + self.pnp_group_tracker = SquareTagGroupPoseTracker( + roles=("t0", "t3", "t4", "t5"), + adjacent_pairs=( + ("t0", "t3"), + ("t3", "t4"), + ("t4", "t5"), + ), + maximum_pose_jump_rad=self.pnp_maximum_pose_jump_rad, + maximum_translation_jump_m=( + self.pnp_maximum_translation_jump_m + ), + relative_rotation_scale_rad=( + self.pnp_group_relative_rotation_scale_rad + ), + relative_translation_scale_m=( + self.pnp_group_relative_translation_scale_m + ), + reprojection_scale_px=( + self.pnp_trajectory_reprojection_scale_px + ), + reprojection_weight=self.pnp_group_reprojection_weight, + reset_after_seconds=self.pnp_tracker_reset_seconds, + ) self.collector = PointCollector( stable_frames=self.stable_frames, capture_frames=self.capture_frames, minimum_settle_seconds=self.minimum_settle_seconds, maximum_stable_spread_rad=self.maximum_stable_spread_rad, + stability_mode=( + "translation" + if self.angle_estimation_mode == "trajectory_center_3d" + else "rotation" + ), + maximum_stable_translation_spread_m=( + self.maximum_stable_translation_spread_m + ), settle_timeout_seconds=self.settle_timeout_seconds, capture_timeout_seconds=self.capture_timeout_seconds, ) @@ -162,6 +240,9 @@ class G20ThumbCalibrationNode(Node): self.status_publisher = self.create_publisher( String, "~/status", 10 ) + self.status_text_publisher = self.create_publisher( + String, "~/status_text", 10 + ) self.debug_publisher = None self.last_debug_publish = 0.0 if self.publish_debug_image: @@ -183,9 +264,8 @@ class G20ThumbCalibrationNode(Node): AprilTagDetectionArray, self.detections_topic, self._detections_callback, - 10, + qos_profile_sensor_data, ) - self.create_subscription(TFMessage, self.tf_topic, self._tf_callback, 50) if self.publish_debug_image: self.create_subscription( Image, @@ -230,7 +310,9 @@ class G20ThumbCalibrationNode(Node): ) self.declare_parameter("detections_topic", "/apriltag/detections") self.declare_parameter("tf_topic", "/tf") - self.declare_parameter("angle_estimation_mode", "image_plane_2d") + self.declare_parameter( + "angle_estimation_mode", "trajectory_center_3d" + ) self.declare_parameter("camera_serial_number", "") self.declare_parameter("rosbag_path", "") self.declare_parameter("publish_debug_image", False) @@ -266,10 +348,61 @@ class G20ThumbCalibrationNode(Node): self.declare_parameter("maximum_static_std_deg", 0.5) self.declare_parameter("pose_outlier_threshold_deg", 5.0) self.declare_parameter("minimum_pose_inlier_rate", 0.95) + self.declare_parameter("pnp_minimum_valid_rate", 0.95) + self.declare_parameter("pnp_maximum_reprojection_error_px", 1.5) + self.declare_parameter("pnp_reprojection_tie_px", 1.5) + self.declare_parameter("pnp_maximum_pose_jump_deg", 35.0) + self.declare_parameter("pnp_maximum_translation_jump_m", 0.04) + self.declare_parameter("pnp_maximum_tag_tilt_deg", 75.0) + self.declare_parameter("pnp_tracker_reset_seconds", 5.0) + self.declare_parameter( + "pnp_group_relative_rotation_scale_deg", 5.0 + ) + self.declare_parameter( + "pnp_group_relative_translation_scale_m", 0.01 + ) + self.declare_parameter("pnp_group_reprojection_weight", 0.05) + self.declare_parameter( + "pnp_trajectory_reprojection_scale_px", 0.1 + ) + self.declare_parameter("pnp_rigid_rotation_scale_deg", 5.0) + self.declare_parameter("pnp_rigid_translation_scale_m", 0.01) + self.declare_parameter( + "pnp_rigid_p95_accepted_drift_deg", 8.0 + ) + self.declare_parameter( + "pnp_rigid_maximum_accepted_drift_deg", 15.0 + ) + self.declare_parameter( + "pnp_rigid_p95_accepted_distance_drift_m", 0.003 + ) + self.declare_parameter( + "pnp_rigid_maximum_accepted_distance_drift_m", 0.006 + ) + self.declare_parameter("trajectory_maximum_plane_rms_m", 0.004) + self.declare_parameter("trajectory_maximum_radial_rms_m", 0.004) + self.declare_parameter("trajectory_minimum_radius_m", 0.005) + self.declare_parameter("trajectory_minimum_arc_deg", 15.0) + self.declare_parameter( + "trajectory_maximum_root_role_disagreement_deg", 5.0 + ) + self.declare_parameter( + "trajectory_maximum_anchor_drift_m", 0.005 + ) + self.declare_parameter("passive_ip_multiplier", 1.02) + self.declare_parameter( + "trajectory_static_translation_outlier_m", 0.005 + ) + self.declare_parameter( + "trajectory_maximum_static_translation_rms_m", 0.002 + ) self.declare_parameter("stable_frames", 5) self.declare_parameter("capture_frames", 8) self.declare_parameter("minimum_settle_seconds", 0.4) - self.declare_parameter("maximum_stable_spread_deg", 0.3) + self.declare_parameter("maximum_stable_spread_deg", 1.0) + self.declare_parameter( + "maximum_stable_translation_spread_m", 0.003 + ) self.declare_parameter("settle_timeout_seconds", 5.0) self.declare_parameter("capture_timeout_seconds", 5.0) self.declare_parameter("validation_command_count", 5) @@ -300,10 +433,19 @@ class G20ThumbCalibrationNode(Node): self.detections_topic = str(value("detections_topic")) self.tf_topic = str(value("tf_topic")) self.angle_estimation_mode = str(value("angle_estimation_mode")) - if self.angle_estimation_mode not in {"image_plane_2d", "pnp_3d"}: + if self.angle_estimation_mode not in { + "image_plane_2d", + "pnp_3d", + "trajectory_center_3d", + }: raise ValueError( - "angle_estimation_mode must be image_plane_2d or pnp_3d" + "angle_estimation_mode must be image_plane_2d, pnp_3d, " + "or trajectory_center_3d" ) + self.uses_pnp = self.angle_estimation_mode in { + "pnp_3d", + "trajectory_center_3d", + } self.camera_serial_number = str(value("camera_serial_number")) self.rosbag_path = str(value("rosbag_path")) self.publish_debug_image = bool(value("publish_debug_image")) @@ -387,12 +529,95 @@ class G20ThumbCalibrationNode(Node): self.minimum_pose_inlier_rate = float( value("minimum_pose_inlier_rate") ) + self.pnp_minimum_valid_rate = float(value("pnp_minimum_valid_rate")) + self.pnp_maximum_reprojection_error_px = float( + value("pnp_maximum_reprojection_error_px") + ) + self.pnp_reprojection_tie_px = float( + value("pnp_reprojection_tie_px") + ) + self.pnp_maximum_pose_jump_rad = math.radians( + float(value("pnp_maximum_pose_jump_deg")) + ) + self.pnp_maximum_translation_jump_m = float( + value("pnp_maximum_translation_jump_m") + ) + self.pnp_maximum_tag_tilt_rad = math.radians( + float(value("pnp_maximum_tag_tilt_deg")) + ) + self.pnp_tracker_reset_seconds = float( + value("pnp_tracker_reset_seconds") + ) + self.pnp_group_relative_rotation_scale_rad = math.radians( + float(value("pnp_group_relative_rotation_scale_deg")) + ) + self.pnp_group_relative_translation_scale_m = float( + value("pnp_group_relative_translation_scale_m") + ) + self.pnp_group_reprojection_weight = float( + value("pnp_group_reprojection_weight") + ) + self.pnp_trajectory_reprojection_scale_px = float( + value("pnp_trajectory_reprojection_scale_px") + ) + self.pnp_rigid_rotation_scale_rad = math.radians( + float(value("pnp_rigid_rotation_scale_deg")) + ) + self.pnp_rigid_translation_scale_m = float( + value("pnp_rigid_translation_scale_m") + ) + self.pnp_rigid_p95_accepted_drift_rad = math.radians( + float(value("pnp_rigid_p95_accepted_drift_deg")) + ) + self.pnp_rigid_maximum_accepted_drift_rad = math.radians( + float(value("pnp_rigid_maximum_accepted_drift_deg")) + ) + self.pnp_rigid_p95_accepted_distance_drift_m = float( + value("pnp_rigid_p95_accepted_distance_drift_m") + ) + self.pnp_rigid_maximum_accepted_distance_drift_m = float( + value("pnp_rigid_maximum_accepted_distance_drift_m") + ) + self.trajectory_maximum_plane_rms_m = float( + value("trajectory_maximum_plane_rms_m") + ) + self.trajectory_maximum_radial_rms_m = float( + value("trajectory_maximum_radial_rms_m") + ) + self.trajectory_minimum_radius_m = float( + value("trajectory_minimum_radius_m") + ) + self.trajectory_minimum_arc_rad = math.radians( + float(value("trajectory_minimum_arc_deg")) + ) + self.trajectory_maximum_root_role_disagreement_rad = math.radians( + float( + value( + "trajectory_maximum_root_role_disagreement_deg" + ) + ) + ) + self.trajectory_maximum_anchor_drift_m = float( + value("trajectory_maximum_anchor_drift_m") + ) + self.passive_ip_multiplier = float( + value("passive_ip_multiplier") + ) + self.trajectory_static_translation_outlier_m = float( + value("trajectory_static_translation_outlier_m") + ) + self.trajectory_maximum_static_translation_rms_m = float( + value("trajectory_maximum_static_translation_rms_m") + ) self.stable_frames = int(value("stable_frames")) self.capture_frames = int(value("capture_frames")) self.minimum_settle_seconds = float(value("minimum_settle_seconds")) self.maximum_stable_spread_rad = math.radians( float(value("maximum_stable_spread_deg")) ) + self.maximum_stable_translation_spread_m = float( + value("maximum_stable_translation_spread_m") + ) self.settle_timeout_seconds = float(value("settle_timeout_seconds")) self.capture_timeout_seconds = float(value("capture_timeout_seconds")) self.validation_command_count = int(value("validation_command_count")) @@ -436,6 +661,13 @@ class G20ThumbCalibrationNode(Node): ) if not 1 <= self.command_step <= 255: raise ValueError("command_step must be in [1, 255]") + if ( + not math.isfinite(self.passive_ip_multiplier) + or not 0.5 <= self.passive_ip_multiplier <= 1.5 + ): + raise ValueError( + "passive_ip_multiplier must be finite and in [0.5, 1.5]" + ) if self.maximum_state_image_skew_ns < 0: raise ValueError("maximum_state_image_skew_ms must be non-negative") if self.continuous_minimum_bins < 3: @@ -464,6 +696,90 @@ class G20ThumbCalibrationNode(Node): raise ValueError("pose_outlier_threshold_deg must be positive") if not 0.0 <= self.minimum_pose_inlier_rate <= 1.0: raise ValueError("minimum_pose_inlier_rate must be in [0, 1]") + if not 0.0 <= self.pnp_minimum_valid_rate <= 1.0: + raise ValueError("pnp_minimum_valid_rate must be in [0, 1]") + if self.pnp_maximum_reprojection_error_px <= 0.0: + raise ValueError( + "pnp_maximum_reprojection_error_px must be positive" + ) + if self.pnp_reprojection_tie_px < 0.0: + raise ValueError("pnp_reprojection_tie_px must be non-negative") + if self.pnp_maximum_pose_jump_rad <= 0.0: + raise ValueError("pnp_maximum_pose_jump_deg must be positive") + if self.pnp_maximum_translation_jump_m <= 0.0: + raise ValueError( + "pnp_maximum_translation_jump_m must be positive" + ) + if not 0.0 < self.pnp_maximum_tag_tilt_rad < math.pi / 2.0: + raise ValueError("pnp_maximum_tag_tilt_deg must be in (0, 90)") + if self.pnp_tracker_reset_seconds <= 0.0: + raise ValueError("pnp_tracker_reset_seconds must be positive") + if self.pnp_group_relative_rotation_scale_rad <= 0.0: + raise ValueError( + "pnp_group_relative_rotation_scale_deg must be positive" + ) + if self.pnp_group_relative_translation_scale_m <= 0.0: + raise ValueError( + "pnp_group_relative_translation_scale_m must be positive" + ) + if self.pnp_group_reprojection_weight < 0.0: + raise ValueError( + "pnp_group_reprojection_weight must be non-negative" + ) + if self.pnp_trajectory_reprojection_scale_px <= 0.0: + raise ValueError( + "pnp_trajectory_reprojection_scale_px must be positive" + ) + if self.pnp_rigid_rotation_scale_rad <= 0.0: + raise ValueError( + "pnp_rigid_rotation_scale_deg must be positive" + ) + if self.pnp_rigid_translation_scale_m <= 0.0: + raise ValueError( + "pnp_rigid_translation_scale_m must be positive" + ) + if self.pnp_rigid_p95_accepted_drift_rad <= 0.0: + raise ValueError( + "pnp_rigid_p95_accepted_drift_deg must be positive" + ) + if self.pnp_rigid_maximum_accepted_drift_rad <= 0.0: + raise ValueError( + "pnp_rigid_maximum_accepted_drift_deg must be positive" + ) + if ( + self.pnp_rigid_p95_accepted_drift_rad + > self.pnp_rigid_maximum_accepted_drift_rad + ): + raise ValueError( + "pnp rigid p95 drift limit must not exceed maximum limit" + ) + if min( + self.pnp_rigid_p95_accepted_distance_drift_m, + self.pnp_rigid_maximum_accepted_distance_drift_m, + ) <= 0.0: + raise ValueError( + "pnp rigid distance drift limits must be positive" + ) + if ( + self.pnp_rigid_p95_accepted_distance_drift_m + > self.pnp_rigid_maximum_accepted_distance_drift_m + ): + raise ValueError( + "pnp rigid p95 distance drift limit must not exceed " + "maximum limit" + ) + if min( + self.trajectory_maximum_plane_rms_m, + self.trajectory_maximum_radial_rms_m, + self.trajectory_minimum_radius_m, + self.trajectory_minimum_arc_rad, + self.trajectory_maximum_root_role_disagreement_rad, + self.trajectory_maximum_anchor_drift_m, + self.trajectory_static_translation_outlier_m, + self.trajectory_maximum_static_translation_rms_m, + self.maximum_stable_translation_spread_m, + ) <= 0.0: + raise ValueError("trajectory centre-fit thresholds must be positive") if not 1 <= self.validation_command_count <= 254: raise ValueError("validation_command_count must be in [1, 254]") if self.validation_approach_minimum_seconds < 0.0: @@ -480,13 +796,38 @@ class G20ThumbCalibrationNode(Node): ) def _camera_info_callback(self, message: CameraInfo) -> None: - self.camera_info_valid = ( + camera_info_valid = ( message.width > 0 and message.height > 0 and len(message.p) == 12 and float(message.p[0]) > 0.0 and float(message.p[5]) > 0.0 ) + new_camera_matrix: np.ndarray | None = None + if camera_info_valid: + projection = np.asarray(message.p, dtype=float).reshape(3, 4) + new_camera_matrix = projection[:, :3].copy() + camera_info_valid = bool( + np.all(np.isfinite(new_camera_matrix)) + and new_camera_matrix[0, 0] > 0.0 + and new_camera_matrix[1, 1] > 0.0 + ) + if ( + new_camera_matrix is not None + and self.pnp_camera_matrix is not None + and not np.allclose( + new_camera_matrix, + self.pnp_camera_matrix, + rtol=1.0e-9, + atol=1.0e-9, + ) + ): + self.pnp_tracker.reset() + self.pnp_group_tracker.reset() + self.camera_info_valid = camera_info_valid + self.pnp_camera_matrix = ( + new_camera_matrix if camera_info_valid else None + ) self.camera_frame = message.header.frame_id self.camera_intrinsics = { "frame_id": message.header.frame_id, @@ -496,6 +837,23 @@ class G20ThumbCalibrationNode(Node): "d": [float(value) for value in message.d], "k": [float(value) for value in message.k], "p": [float(value) for value in message.p], + "rectified_camera_matrix": ( + [] + if self.pnp_camera_matrix is None + else [ + float(value) + for value in self.pnp_camera_matrix.reshape(-1) + ] + ), + "binning_x": int(message.binning_x), + "binning_y": int(message.binning_y), + "roi": { + "x_offset": int(message.roi.x_offset), + "y_offset": int(message.roi.y_offset), + "height": int(message.roi.height), + "width": int(message.roi.width), + "do_rectify": bool(message.roi.do_rectify), + }, } def _state_callback(self, message: JointState) -> None: @@ -548,45 +906,127 @@ class G20ThumbCalibrationNode(Node): edge_pixels=float(np.mean(edges)), ) latest_corners[role] = corners + + transforms: dict[str, tuple[float, ...]] = {} + translations: dict[str, tuple[float, float, float]] = {} + pose_candidates: dict[str, tuple[SquareTagPose, ...]] = {} + pnp_rejections: dict[str, str] = {} + pnp_errors: dict[str, float] = {} + if self.uses_pnp: + if self.pnp_camera_matrix is None: + pnp_rejections = { + role: "camera_info_not_ready" + for role in self.tag_config + } + else: + for role in self.tag_config: + corners = latest_corners.get(role) + if corners is None: + pnp_rejections[role] = "tag_not_detected" + continue + quality = qualities.get(role) + if quality is None or not self._quality_is_valid( + quality, + include_pnp=False, + ): + pnp_rejections[role] = "tag_quality_invalid" + continue + pose, reason = self.pnp_tracker.estimate( + role, + corners, + tag_size_m=float( + self.tag_config[role]["size_m"] + ), + camera_matrix=self.pnp_camera_matrix, + stamp_ns=stamp, + reprojection_tie_px=( + self._pnp_reprojection_tie_for_role(role) + ), + ) + pose_candidates[role] = ( + self.pnp_tracker.last_candidates_by_role.get( + role, + (), + ) + ) + if pose is None and not pose_candidates[role]: + pnp_rejections[role] = reason + if ( + set(pose_candidates) == set(self.tag_config) + and all(pose_candidates.values()) + ): + selected_group, group_reason = ( + self.pnp_group_tracker.select( + pose_candidates, + stamp_ns=stamp, + ) + ) + if selected_group is None: + pnp_rejections["_group"] = group_reason + else: + transforms.clear() + pnp_errors.clear() + pnp_rejections.clear() + for role, pose in selected_group.items(): + transforms[role] = pose.quaternion_xyzw + translations[role] = pose.translation_xyz_m + pnp_errors[role] = pose.reprojection_error_px + quality = qualities[role] + qualities[role] = replace( + quality, + reprojection_error_px=( + pose.reprojection_error_px + ), + ) + self.transform_cache[stamp] = transforms + self.translation_cache[stamp] = translations + self.pnp_candidate_cache[stamp] = pose_candidates + self.preflight_pnp_flags.append( + set(transforms) == set(self.tag_config) + ) + self.latest_pnp_rejections = pnp_rejections + self.latest_pnp_errors_px = pnp_errors + self.latest_corners = latest_corners + self.latest_tag_qualities = dict(qualities) self.quality_cache[stamp] = qualities self.corner_cache[stamp] = latest_corners - all_good = set(qualities) == set(self.tag_config) and all( - self._quality_is_valid(quality) for quality in qualities.values() + detection_good = set(qualities) == set(self.tag_config) and all( + self._quality_is_valid(quality, include_pnp=False) + for quality in qualities.values() ) - self.preflight_detection_flags.append(all_good) + all_good = detection_good and all( + self._quality_is_valid(quality, include_pnp=True) + for quality in qualities.values() + ) + self.preflight_detection_flags.append(detection_good) self.preflight_detection_times.append(time.monotonic()) if not all_good and self.collector.active: self.collector.mark_invalid_frame() self._try_emit_observation(stamp) self._trim_caches() - def _tf_callback(self, message: TFMessage) -> None: - touched: set[int] = set() - for transform in message.transforms: - role = self.role_by_frame.get(transform.child_frame_id.lstrip("/")) - if role is None: - continue - stamp = _stamp_ns(transform.header.stamp) - rotation = transform.transform.rotation - quaternion = ( - float(rotation.x), - float(rotation.y), - float(rotation.z), - float(rotation.w), - ) - self.transform_cache.setdefault(stamp, {})[role] = quaternion - touched.add(stamp) - for stamp in touched: - self._try_emit_observation(stamp) - self._trim_caches() + def _pnp_reprojection_tie_for_role(self, role: str) -> float: + """Lock every visible tag to its temporally continuous IPPE branch.""" + del role + return self.pnp_reprojection_tie_px - def _quality_is_valid(self, quality: TagQuality) -> bool: + def _quality_is_valid( + self, + quality: TagQuality, + *, + include_pnp: bool = True, + ) -> bool: return tag_quality_is_valid( quality, maximum_hamming=self.maximum_hamming, minimum_decision_margin=self.minimum_decision_margin, minimum_edge_pixels=self.minimum_edge_pixels, + maximum_reprojection_error_px=( + self.pnp_maximum_reprojection_error_px + if include_pnp and self.uses_pnp + else None + ), ) def _try_emit_observation(self, stamp: int) -> None: @@ -612,6 +1052,12 @@ class G20ThumbCalibrationNode(Node): transforms = self.transform_cache.get(stamp) if transforms is None or set(transforms) != required: return + translations = self.translation_cache.get(stamp, {}) + if ( + self.angle_estimation_mode == "trajectory_center_3d" + and set(translations) != required + ): + return relative = { PAIR_ROOT: relative_quaternion_xyzw( transforms["t0"], transforms["t3"] @@ -643,6 +1089,9 @@ class G20ThumbCalibrationNode(Node): state_u8=state_u8, state_stamp_ns=state_stamp_ns, state_sync_error_ns=state_sync_error_ns, + tag_quaternion_xyzw=transforms, + tag_translation_xyz_m=translations, + tag_pose_candidates=self.pnp_candidate_cache.get(stamp, {}), ) self.emitted_stamps.add(stamp) self.preflight_observations.append(observation) @@ -660,11 +1109,15 @@ class G20ThumbCalibrationNode(Node): set(self.quality_cache) | set(self.corner_cache) | set(self.transform_cache) + | set(self.translation_cache) + | set(self.pnp_candidate_cache) ) for stamp in all_stamps[:-300]: self.quality_cache.pop(stamp, None) self.corner_cache.pop(stamp, None) self.transform_cache.pop(stamp, None) + self.translation_cache.pop(stamp, None) + self.pnp_candidate_cache.pop(stamp, None) self.emitted_stamps.discard(stamp) def _image_callback(self, message: Image) -> None: @@ -816,6 +1269,16 @@ class G20ThumbCalibrationNode(Node): ) if detection_hz < self.minimum_detection_hz: return False, f"detection_hz_too_low:{detection_hz:.2f}" + if self.uses_pnp: + if len(self.preflight_pnp_flags) < self.preflight_frames: + return ( + False, + "collecting_pnp_preflight:" + f"{len(self.preflight_pnp_flags)}/{self.preflight_frames}", + ) + pnp_valid_rate = float(np.mean(self.preflight_pnp_flags)) + if pnp_valid_rate < self.pnp_minimum_valid_rate: + return False, f"pnp_valid_rate_too_low:{pnp_valid_rate:.3f}" if len(self.preflight_observations) < self.preflight_frames: return ( False, @@ -823,25 +1286,67 @@ class G20ThumbCalibrationNode(Node): f"{len(self.preflight_observations)}/{self.preflight_frames}", ) observations = list(self.preflight_observations) - for pair in PAIR_NAMES: - quaternions = [ - observation.relative_quaternion_xyzw[pair] - for observation in observations - ] - inlier_rate = rotation_inlier_fraction( - quaternions, - outlier_threshold_rad=self.pose_outlier_threshold_rad, - ) - if inlier_rate < self.minimum_pose_inlier_rate: - return False, ( - f"{pair}_pose_inlier_rate_too_low:{inlier_rate:.3f}" + if self.angle_estimation_mode == "trajectory_center_3d": + for pair, (parent, child) in TAG_PAIR_ROLES.items(): + 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, ) - rms = rotation_rms_rad( - quaternions, - outlier_threshold_rad=self.pose_outlier_threshold_rad, - ) - if rms > self.maximum_static_std_rad: - return False, f"{pair}_static_rms_too_large:{rms:.6f}" + reference = np.median(vectors, axis=0) + residuals = np.linalg.norm(vectors - reference, axis=1) + inliers = ( + residuals + <= self.trajectory_static_translation_outlier_m + ) + inlier_rate = float(np.mean(inliers)) + if inlier_rate < self.minimum_pose_inlier_rate: + return False, ( + f"{pair}_position_inlier_rate_too_low:" + f"{inlier_rate:.3f}" + ) + rms = float( + np.sqrt(np.mean(np.square(residuals[inliers]))) + ) + if ( + rms + > self.trajectory_maximum_static_translation_rms_m + ): + return False, ( + f"{pair}_static_position_rms_too_large:" + f"{1000.0 * rms:.3f}mm" + ) + else: + for pair in PAIR_NAMES: + quaternions = [ + observation.relative_quaternion_xyzw[pair] + for observation in observations + ] + inlier_rate = rotation_inlier_fraction( + quaternions, + outlier_threshold_rad=self.pose_outlier_threshold_rad, + ) + if inlier_rate < self.minimum_pose_inlier_rate: + return False, ( + f"{pair}_pose_inlier_rate_too_low:{inlier_rate:.3f}" + ) + rms = rotation_rms_rad( + quaternions, + outlier_threshold_rad=self.pose_outlier_threshold_rad, + ) + if rms > self.maximum_static_std_rad: + return False, ( + f"{pair}_static_rms_too_large:{rms:.6f}" + ) return True, "preflight_passed" def _has_other_command_publishers(self) -> bool: @@ -857,6 +1362,34 @@ class G20ThumbCalibrationNode(Node): def _select_post_preflight_state(self) -> None: observations = list(self.preflight_observations) + static_position_rms_m: dict[str, float] = {} + position_inlier_rate: dict[str, float] = {} + if self.angle_estimation_mode == "trajectory_center_3d": + for pair, (parent, child) in TAG_PAIR_ROLES.items(): + 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) + residuals = np.linalg.norm(vectors - reference, axis=1) + inliers = ( + residuals + <= self.trajectory_static_translation_outlier_m + ) + position_inlier_rate[pair] = float(np.mean(inliers)) + static_position_rms_m[pair] = float( + np.sqrt(np.mean(np.square(residuals[inliers]))) + ) self._write_manifest( { "preflight": { @@ -871,6 +1404,26 @@ class G20ThumbCalibrationNode(Node): - self.preflight_detection_times[0] ) ), + "pnp_valid_rate": ( + float(np.mean(self.preflight_pnp_flags)) + if self.uses_pnp + and self.preflight_pnp_flags + else None + ), + "maximum_reprojection_error_px": { + role: float( + max( + observation.tag_quality[ + role + ].reprojection_error_px + or 0.0 + for observation in observations + ) + ) + for role in self.tag_config + } + if self.uses_pnp + else {}, "static_rms_rad": { pair: rotation_rms_rad( [ @@ -891,6 +1444,8 @@ class G20ThumbCalibrationNode(Node): ) for pair in PAIR_NAMES }, + "static_position_rms_m": static_position_rms_m, + "position_inlier_rate": position_inlier_rate, } } ) @@ -1218,7 +1773,17 @@ class G20ThumbCalibrationNode(Node): f"{task['phase']}:validation:approach_wait:u={command}" ) return - self.collector.start(time.monotonic()) + point_tolerance = ( + self.validation_position_tolerance_u8 + if task.get("kind") == "validation" + else self.continuous_endpoint_tolerance_u8 + ) + self.collector.start( + time.monotonic(), + required_state_index=motor_index, + required_state_u8=command, + maximum_state_error_u8=point_tolerance, + ) self.state_reason = ( f"{task['phase']}:{task.get('kind')}:{task.get('stage')}:u={command}" ) @@ -1271,10 +1836,60 @@ class G20ThumbCalibrationNode(Node): raise RuntimeError(f"unknown task kind {task.get('kind')}") if task.get("stage") == "approach": raise RuntimeError("validation approach must not capture images") + motor_index = 0 if task["phase"] == PHASE_ROOT else 15 + state_median = aggregate.get("state_u8_median", []) + if len(state_median) != 20: + self._pause("validation_capture_missing_synchronised_state") + return + actual = float(state_median[motor_index]) + target = float(task["command_u8"]) + if ( + not math.isfinite(actual) + or abs(actual - target) + > self.validation_position_tolerance_u8 + ): + self._pause( + "validation_capture_position_mismatch:" + f"target={target:.1f},actual={actual:.1f}" + ) + return + if self.angle_estimation_mode == "trajectory_center_3d": + maximum_capture_spread = max( + ( + float(value) + for value in aggregate.get( + "maximum_translation_spread_m", {} + ).values() + ), + default=float("inf"), + ) + if ( + maximum_capture_spread + > self.maximum_stable_translation_spread_m + ): + self._pause( + "validation_capture_position_spread_too_large:" + f"{1000.0 * maximum_capture_spread:.2f}mm" + ) + return + else: + maximum_capture_spread = max( + ( + float(value) + for value in aggregate.get( + "maximum_spread_rad", {} + ).values() + ), + default=float("inf"), + ) + if maximum_capture_spread > self.maximum_stable_spread_rad: + self._pause( + "validation_capture_pose_spread_too_large:" + f"{math.degrees(maximum_capture_spread):.2f}deg" + ) + return if task.get("zero_reference"): - self.validation_zero[task["phase"]] = aggregate[ - "relative_quaternion_xyzw" - ] + self.validation_zero[task["phase"]] = aggregate else: self._record_validation(task, aggregate) self.active_task = None @@ -1320,7 +1935,14 @@ class G20ThumbCalibrationNode(Node): ) if at_target: task["stage"] = "prepare_capture" - self.collector.start(float(now)) + self.collector.start( + float(now), + required_state_index=motor_index, + required_state_u8=command, + maximum_state_error_u8=( + self.continuous_endpoint_tolerance_u8 + ), + ) self.state_reason = ( f"{task['phase']}:continuous:prepare_capture:u={command}" ) @@ -1374,6 +1996,136 @@ class G20ThumbCalibrationNode(Node): f"target={command},actual={actual:.1f}" ) + def _resolve_pnp_trajectory_branches( + self, + observations: list[Observation], + *, + phase: str, + ) -> tuple[list[Observation], dict[str, Any]]: + if not self.uses_pnp: + return observations, {} + if phase == PHASE_ROOT: + rigid_roles = ("t3", "t4", "t5") + fixed_pairs = (("t3", "t4"), ("t4", "t5")) + # Motor 0 moves T3/T4/T5 as one rigid chain. Do not freeze T3 to + # its online IPPE branch: if T3 is the ambiguous tag, fixing it + # forces the optimiser to flip T4/T5 merely to compensate and + # leaves a large rigid drift. The two rigid pairs provide enough + # information to resolve all three branches jointly. + fixed_online_roles = () + elif phase == PHASE_TIP: + rigid_roles = ("t0", "t3") + fixed_pairs = (("t0", "t3"),) + # T3 is the parent reference for the active MCP trajectory. Do + # not let a non-target root constraint rewrite that active curve. + fixed_online_roles = ("t3",) + else: + raise ValueError(f"unknown calibration phase {phase}") + + frames = [] + for observation in observations: + frame = dict(observation.tag_pose_candidates) + for role in fixed_online_roles: + online_quaternion = observation.tag_quaternion_xyzw.get( + role + ) + candidates = tuple(frame.get(role, ())) + if online_quaternion is None or not candidates: + raise ValueError( + f"trajectory frame is missing fixed role {role}" + ) + frame[role] = ( + min( + candidates, + key=lambda candidate: rotation_distance_rad( + online_quaternion, + candidate.quaternion_xyzw, + ), + ), + ) + frames.append(frame) + if any( + any(not frame.get(role) for role in rigid_roles) + for frame in frames + ): + raise ValueError("trajectory frame is missing PnP candidates") + selected_path, quality = select_rigid_group_trajectory( + frames, + roles=rigid_roles, + fixed_pairs=fixed_pairs, + reprojection_scale_px=( + self.pnp_trajectory_reprojection_scale_px + ), + rotation_scale_rad=self.pnp_rigid_rotation_scale_rad, + translation_scale_m=self.pnp_rigid_translation_scale_m, + pair_geometry=( + "distance" + if self.angle_estimation_mode == "trajectory_center_3d" + else "pose" + ), + ) + + corrected: list[Observation] = [] + corrected_role_poses = 0 + corrected_frames = 0 + for observation, selected in zip(observations, selected_path): + transforms = dict(observation.tag_quaternion_xyzw) + translations = dict(observation.tag_translation_xyz_m) + tag_quality = dict(observation.tag_quality) + frame_corrected = False + for role, pose in selected.items(): + original = transforms.get(role) + if ( + original is not None + and rotation_distance_rad( + original, + pose.quaternion_xyzw, + ) + > 1.0e-6 + ): + corrected_role_poses += 1 + frame_corrected = True + transforms[role] = pose.quaternion_xyzw + translations[role] = pose.translation_xyz_m + if role in tag_quality: + tag_quality[role] = replace( + tag_quality[role], + reprojection_error_px=( + pose.reprojection_error_px + ), + ) + if frame_corrected: + corrected_frames += 1 + relative = { + PAIR_ROOT: relative_quaternion_xyzw( + transforms["t0"], transforms["t3"] + ), + PAIR_MCP: relative_quaternion_xyzw( + transforms["t3"], transforms["t4"] + ), + PAIR_IP: relative_quaternion_xyzw( + transforms["t4"], transforms["t5"] + ), + } + corrected.append( + replace( + observation, + relative_quaternion_xyzw=relative, + tag_quality=tag_quality, + tag_quaternion_xyzw=transforms, + tag_translation_xyz_m=translations, + ) + ) + report: dict[str, Any] = { + **quality, + "phase": phase, + "frames": len(observations), + "corrected_frames": corrected_frames, + "corrected_role_poses": corrected_role_poses, + "fixed_online_roles": list(fixed_online_roles), + } + return corrected, report + def _complete_continuous_sweep( self, observations: list[Observation] ) -> None: @@ -1384,6 +2136,74 @@ class G20ThumbCalibrationNode(Node): or task.get("stage") != "sweep" ): return + try: + observations, trajectory_report = ( + self._resolve_pnp_trajectory_branches( + observations, + phase=str(task["phase"]), + ) + ) + except Exception as error: + self.sweep_collector.state = "idle" + self._pause(f"pnp_trajectory_selection_failed:{error}") + return + if trajectory_report: + trajectory_report["direction"] = str(task["direction"]) + self.latest_trajectory_branch_quality = trajectory_report + maximum_rigid_drift = float( + trajectory_report.get( + "maximum_pair_rotation_drift_rad", + float("inf"), + ) + ) + p95_rigid_drift = float( + trajectory_report.get( + "p95_pair_rotation_drift_rad", + float("inf"), + ) + ) + if self.angle_estimation_mode == "trajectory_center_3d": + maximum_translation_drift = float( + trajectory_report.get( + "maximum_pair_distance_drift_m", + float("inf"), + ) + ) + p95_translation_drift = float( + trajectory_report.get( + "p95_pair_distance_drift_m", + float("inf"), + ) + ) + if ( + p95_translation_drift + > self.pnp_rigid_p95_accepted_distance_drift_m + or maximum_translation_drift + > self.pnp_rigid_maximum_accepted_distance_drift_m + ): + self.sweep_collector.state = "idle" + self._pause( + "pnp_trajectory_rigid_distance_drift_too_large:" + f"p95={1000.0 * p95_translation_drift:.2f}mm," + "max=" + f"{1000.0 * maximum_translation_drift:.2f}mm" + ) + return + elif ( + p95_rigid_drift + > self.pnp_rigid_p95_accepted_drift_rad + or maximum_rigid_drift + > self.pnp_rigid_maximum_accepted_drift_rad + ): + self.sweep_collector.state = "idle" + self._pause( + "pnp_trajectory_rigid_drift_too_large:" + f"p95={math.degrees(p95_rigid_drift):.2f}deg," + "max=" + f"{math.degrees(maximum_rigid_drift):.2f}deg" + ) + return + self.trajectory_branch_reports.append(trajectory_report) motor_index = 0 if task["phase"] == PHASE_ROOT else 15 aggregates = aggregate_sweep_observations( observations, @@ -1394,7 +2214,10 @@ class G20ThumbCalibrationNode(Node): ) start_command = int(task["start_u8"]) start_aggregate = task.get("start_aggregate") - if isinstance(start_aggregate, dict): + if ( + isinstance(start_aggregate, dict) + and start_command not in aggregates + ): aggregates[start_command] = start_aggregate commands = sorted(aggregates) @@ -1423,6 +2246,7 @@ class G20ThumbCalibrationNode(Node): "command_u8": int(command), "sweep_start_u8": int(task["start_u8"]), "sweep_target_u8": int(task["target_u8"]), + "pnp_trajectory_quality": trajectory_report, **aggregates[command], } append_jsonl(self.raw_path, record) @@ -1430,13 +2254,49 @@ class G20ThumbCalibrationNode(Node): self.active_task = None self.sweep_collector.state = "idle" self._write_checkpoint() + if trajectory_report: + self._write_manifest( + { + "pnp_trajectory_branch_reports": ( + self.trajectory_branch_reports + ) + } + ) + + def _fit_calibration_records(self) -> FitResult: + if self.angle_estimation_mode != "trajectory_center_3d": + return fit_calibration_curves(self.records) + return fit_center_trajectory_curves( + self.records, + passive_ip_multiplier=self.passive_ip_multiplier, + maximum_plane_rms_m=self.trajectory_maximum_plane_rms_m, + maximum_radial_rms_m=self.trajectory_maximum_radial_rms_m, + minimum_radius_m=self.trajectory_minimum_radius_m, + minimum_arc_rad=self.trajectory_minimum_arc_rad, + maximum_root_role_disagreement_rad=( + self.trajectory_maximum_root_role_disagreement_rad + ), + maximum_anchor_drift_m=( + self.trajectory_maximum_anchor_drift_m + ), + ) def _begin_validation(self) -> None: try: - self.fit = fit_calibration_curves(self.records) + self.fit = self._fit_calibration_records() except Exception as error: self._pause(f"curve_fit_failed:{error}") return + if self.fit.trajectory_quality: + self._write_manifest( + { + "trajectory_center_fit": { + "zero_command_u8": 255, + "models": self.fit.trajectory_models, + "quality": self.fit.trajectory_quality, + } + } + ) generator = random.Random(self.validation_seed) commands = sorted( generator.sample(range(1, 255), self.validation_command_count) @@ -1505,6 +2365,19 @@ class G20ThumbCalibrationNode(Node): zero = self.validation_zero.get(phase) if zero is None: raise RuntimeError(f"missing validation zero for {phase}") + centre_measurements: dict[str, float] = {} + centre_zero: dict[str, float] = {} + if self.fit.measurement_mode == "trajectory_center_3d": + centre_measurements = measure_center_trajectory_phase_angles( + self.fit.trajectory_models, + aggregate["tag_translation_xyz_m"], + phase=phase, + ) + centre_zero = measure_center_trajectory_phase_angles( + self.fit.trajectory_models, + zero["tag_translation_xyz_m"], + phase=phase, + ) measurements: dict[str, float] = {} predictions: dict[str, float] = {} errors: dict[str, float] = {} @@ -1519,11 +2392,17 @@ class G20ThumbCalibrationNode(Node): "thumb_mcp": PAIR_MCP, "thumb_ip": PAIR_IP, }[joint_name] - measurement = self.fit.measure_from_reference( - joint_name, - aggregate["relative_quaternion_xyzw"][pair], - zero[pair], - ) + if self.fit.measurement_mode == "trajectory_center_3d": + measurement = ( + centre_measurements[joint_name] + - centre_zero[joint_name] + ) + else: + measurement = self.fit.measure_from_reference( + joint_name, + aggregate["relative_quaternion_xyzw"][pair], + zero["relative_quaternion_xyzw"][pair], + ) prediction = float( self.fit.joints[joint_name]["angle_rad"][command] ) @@ -1547,11 +2426,18 @@ class G20ThumbCalibrationNode(Node): def _finalize(self) -> None: if self.fit is None: - self.fit = fit_calibration_curves(self.records) + self.fit = self._fit_calibration_records() errors = np.abs(np.asarray(self.validation_errors, dtype=float)) mae = float(np.mean(errors)) if errors.size else float("inf") p95 = float(np.percentile(errors, 95)) if errors.size else float("inf") - coupling_drift = maximum_non_target_drift_rad(self.records, self.fit) + coupling_drift = ( + maximum_center_non_target_drift_rad( + self.records, + self.fit.trajectory_models, + ) + if self.fit.measurement_mode == "trajectory_center_3d" + else maximum_non_target_drift_rad(self.records, self.fit) + ) passed = ( errors.size > 0 and mae <= self.maximum_validation_mae_rad @@ -1652,6 +2538,14 @@ class G20ThumbCalibrationNode(Node): "existing session calibration speed does not match; " "start a new session" ) + existing_mode = existing.get("camera", {}).get( + "angle_estimation_mode", "pnp_3d" + ) + if existing_mode != self.angle_estimation_mode: + raise ValueError( + "existing session angle estimation mode does not match; " + "start a new session" + ) capture = existing.get("capture", {}) if capture.get("scan_mode", "point") != self.scan_mode: raise ValueError( @@ -1698,6 +2592,84 @@ class G20ThumbCalibrationNode(Node): "image_topic": self.image_topic, "depth_used_for_angle": False, "angle_estimation_mode": self.angle_estimation_mode, + "pnp": { + "solver": "opencv_solvePnPGeneric_IPPE_SQUARE", + "input_is_rectified": True, + "uses_detection_corners_directly": True, + "maximum_reprojection_error_px": ( + self.pnp_maximum_reprojection_error_px + ), + "reprojection_tie_px": self.pnp_reprojection_tie_px, + "maximum_pose_jump_rad": ( + self.pnp_maximum_pose_jump_rad + ), + "maximum_translation_jump_m": ( + self.pnp_maximum_translation_jump_m + ), + "maximum_tag_tilt_rad": ( + self.pnp_maximum_tag_tilt_rad + ), + "tracker_reset_seconds": ( + self.pnp_tracker_reset_seconds + ), + "group_relative_rotation_scale_rad": ( + self.pnp_group_relative_rotation_scale_rad + ), + "group_relative_translation_scale_m": ( + self.pnp_group_relative_translation_scale_m + ), + "group_reprojection_weight": ( + self.pnp_group_reprojection_weight + ), + "trajectory_reprojection_scale_px": ( + self.pnp_trajectory_reprojection_scale_px + ), + "rigid_rotation_scale_rad": ( + self.pnp_rigid_rotation_scale_rad + ), + "rigid_translation_scale_m": ( + self.pnp_rigid_translation_scale_m + ), + "rigid_p95_accepted_drift_rad": ( + self.pnp_rigid_p95_accepted_drift_rad + ), + "rigid_maximum_accepted_drift_rad": ( + self.pnp_rigid_maximum_accepted_drift_rad + ), + "rigid_p95_accepted_distance_drift_m": ( + self.pnp_rigid_p95_accepted_distance_drift_m + ), + "rigid_maximum_accepted_distance_drift_m": ( + self.pnp_rigid_maximum_accepted_distance_drift_m + ), + }, + "trajectory_center_3d": { + "uses_tag_centres": True, + "palm_tag_is_translation_anchor": True, + "zero_command_u8": 255, + "passive_ip_estimation": "mcp_mimic_constraint", + "passive_ip_multiplier": self.passive_ip_multiplier, + "maximum_plane_rms_m": ( + self.trajectory_maximum_plane_rms_m + ), + "maximum_radial_rms_m": ( + self.trajectory_maximum_radial_rms_m + ), + "minimum_radius_m": self.trajectory_minimum_radius_m, + "minimum_arc_rad": self.trajectory_minimum_arc_rad, + "maximum_root_role_disagreement_rad": ( + self.trajectory_maximum_root_role_disagreement_rad + ), + "maximum_anchor_drift_m": ( + self.trajectory_maximum_anchor_drift_m + ), + "static_translation_outlier_m": ( + self.trajectory_static_translation_outlier_m + ), + "maximum_static_translation_rms_m": ( + self.trajectory_maximum_static_translation_rms_m + ), + }, "debug_image_enabled": self.publish_debug_image, "intrinsics": self.camera_intrinsics, }, @@ -1720,8 +2692,12 @@ class G20ThumbCalibrationNode(Node): "capture_frames": self.capture_frames, "minimum_settle_seconds": self.minimum_settle_seconds, "maximum_stable_spread_rad": self.maximum_stable_spread_rad, + "maximum_stable_translation_spread_m": ( + self.maximum_stable_translation_spread_m + ), "pose_outlier_threshold_rad": self.pose_outlier_threshold_rad, "minimum_pose_inlier_rate": self.minimum_pose_inlier_rate, + "pnp_minimum_valid_rate": self.pnp_minimum_valid_rate, "maximum_state_image_skew_ms": ( self.maximum_state_image_skew_ns / 1_000_000.0 ), @@ -1759,7 +2735,15 @@ class G20ThumbCalibrationNode(Node): def _code_sha256() -> str: digest = hashlib.sha256() source_dir = Path(__file__).resolve().parent - for name in ("acquisition.py", "core.py", "node.py", "storage.py"): + for name in ( + "acquisition.py", + "core.py", + "diagnostics.py", + "node.py", + "pnp.py", + "storage.py", + "trajectory.py", + ): path = source_dir / name digest.update(name.encode("utf-8")) digest.update(path.read_bytes()) @@ -1789,12 +2773,63 @@ class G20ThumbCalibrationNode(Node): scan_targets(self.repetitions, self.command_step) ) * 2 scan_progress = completed_scan / total_scan + tag_quality = build_tag_quality_diagnostics( + self.tag_config, + self.latest_tag_qualities, + self.latest_pnp_rejections, + self.latest_pnp_errors_px, + maximum_hamming=self.maximum_hamming, + minimum_decision_margin=self.minimum_decision_margin, + minimum_edge_pixels=self.minimum_edge_pixels, + maximum_reprojection_error_px=( + self.pnp_maximum_reprojection_error_px + ), + ) + reason_zh, action_zh = status_guidance_zh( + self.state, + self.state_reason, + tag_quality, + ) payload = { "state": self.state, + "state_zh": STATE_NAMES_ZH.get(self.state, self.state), "reason": self.state_reason, + "reason_zh": reason_zh, + "action_zh": action_zh, "commands_enabled": self.commands_enabled, "calibration_speed_u8": self.calibration_speed, "angle_estimation_mode": self.angle_estimation_mode, + "passive_ip_estimation": ( + "mcp_mimic_constraint" + if self.angle_estimation_mode == "trajectory_center_3d" + else None + ), + "passive_ip_multiplier": ( + self.passive_ip_multiplier + if self.angle_estimation_mode == "trajectory_center_3d" + else None + ), + "pnp_valid_rate": ( + float(np.mean(self.preflight_pnp_flags)) + if self.preflight_pnp_flags + else None + ), + "pnp_rejections": self.latest_pnp_rejections, + "pnp_rejections_zh": { + role: pnp_rejection_zh(reason) + for role, reason in self.latest_pnp_rejections.items() + }, + "pnp_reprojection_error_px": self.latest_pnp_errors_px, + "tag_quality": tag_quality, + "pnp_branch_corrections": ( + self.pnp_group_tracker.branch_correction_counts + ), + "pnp_trajectory_quality": ( + self.latest_trajectory_branch_quality + ), + "trajectory_center_quality": ( + {} if self.fit is None else self.fit.trajectory_quality + ), "scan_mode": self.scan_mode, "continuous_motion_mode": self.continuous_motion_mode, "command_step": self.command_step, @@ -1806,6 +2841,14 @@ class G20ThumbCalibrationNode(Node): "collector_state": self.collector.state, "collector_reason": self.collector.reason, "stable_frames_seen": self.collector.stable_frames_seen, + "stable_spread_deg": { + pair: math.degrees(spread) + for pair, spread in self.collector.stable_spread_rad.items() + }, + "stable_spread_mm": { + pair: 1000.0 * spread + for pair, spread in self.collector.stable_spread_m.items() + }, "capture_frames_seen": self.collector.capture_frames_seen, "sweep_collector_state": self.sweep_collector.state, "sweep_collector_reason": self.sweep_collector.reason, @@ -1824,6 +2867,14 @@ class G20ThumbCalibrationNode(Node): message = String() message.data = json.dumps(payload, ensure_ascii=False) self.status_publisher.publish(message) + text_message = String() + text_message.data = render_status_text_zh( + payload["state_zh"], + reason_zh, + action_zh, + tag_quality, + ) + self.status_text_publisher.publish(text_message) def main(args: list[str] | None = None) -> None: diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/pnp.py new file mode 100644 index 0000000..96054d8 --- /dev/null +++ b/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), "" diff --git a/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/trajectory.py b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/trajectory.py new file mode 100644 index 0000000..f6721fb --- /dev/null +++ b/src/g20_thumb_apriltag_calibration/g20_thumb_apriltag_calibration/trajectory.py @@ -0,0 +1,1046 @@ +"""Joint angles from the 3-D trajectories of AprilTag centres. + +The small planar tags used on the G20 thumb have an ambiguous PnP orientation, +but their estimated centres are considerably more stable. This module uses +those centres as points carried by the links: + +* T3/T4/T5 share one rigid rotation during the CMC sweep. +* T4 traces the MCP circle relative to the stationary T3 during the tip sweep. +* T5 is first de-rotated by the measured MCP motion; its remaining circle is a + diagnostic for vision/linkage consistency. +* The runtime passive IP angle follows the G20 URDF mimic constraint + ``q_ip = 1.02 * q_mcp``. A single coupled motor sweep cannot robustly + identify both serial rotations from one small distal-point trajectory. + +All reported angles are relative to the command-255 observations. +""" + +from __future__ import annotations + +import math +from typing import Any, Iterable, Mapping, Sequence + +import numpy as np +from scipy.optimize import least_squares +from scipy.spatial.transform import Rotation + +from .core import ( + DIRECTION_DECREASING, + DIRECTION_INCREASING, + DIRECTIONS, + FitResult, + PHASE_ROOT, + PHASE_TIP, + isotonic_nonincreasing, +) + + +_ROLES = ("t0", "t3", "t4", "t5") +_ROOT_ROLES = ("t3", "t4", "t5") +DEFAULT_PASSIVE_IP_MULTIPLIER = 1.02 + + +def _vector3(value: Sequence[float], *, name: str) -> np.ndarray: + vector = np.asarray(value, dtype=float) + if vector.shape != (3,) or not np.all(np.isfinite(vector)): + raise ValueError(f"{name} must contain three finite values") + return vector + + +def _record_translations( + record: Mapping[str, Any], +) -> dict[str, np.ndarray]: + values = record.get("tag_translation_xyz_m", {}) + if not isinstance(values, Mapping): + raise ValueError("sample record is missing tag_translation_xyz_m") + missing = sorted(set(_ROLES) - set(values)) + if missing: + raise ValueError( + "sample record is missing tag centres: " + ",".join(missing) + ) + return { + role: _vector3(values[role], name=f"{role} translation") + for role in _ROLES + } + + +def _plane_basis(axis: Sequence[float]) -> tuple[np.ndarray, np.ndarray]: + normal = _vector3(axis, name="axis") + normal /= np.linalg.norm(normal) + candidates = np.eye(3) + seed = candidates[int(np.argmin(np.abs(candidates @ normal)))] + first = np.cross(normal, seed) + first /= np.linalg.norm(first) + second = np.cross(normal, first) + second /= np.linalg.norm(second) + return first, second + + +def _fit_plane_axis(point_sets: Sequence[np.ndarray]) -> tuple[np.ndarray, float]: + centred: list[np.ndarray] = [] + for points in point_sets: + array = np.asarray(points, dtype=float) + if array.ndim != 2 or array.shape[1] != 3 or len(array) < 3: + raise ValueError("each trajectory must contain at least three 3-D points") + # The arithmetic centroid of points in a plane remains in that plane + # and transforms correctly under a camera rotation. A component-wise + # median generally does neither. + centred.append(array - np.mean(array, axis=0)) + matrix = np.concatenate(centred, axis=0) + _, _, vh = np.linalg.svd(matrix, full_matrices=False) + axis = vh[-1] + axis /= np.linalg.norm(axis) + residuals = matrix @ axis + plane_rms = float(np.sqrt(np.mean(np.square(residuals)))) + return axis, plane_rms + + +def _fit_circle_with_axis( + points_xyz: Sequence[Sequence[float]], + axis_xyz: Sequence[float], +) -> dict[str, Any]: + points = np.asarray(points_xyz, dtype=float) + if ( + points.ndim != 2 + or points.shape[1] != 3 + or len(points) < 6 + or not np.all(np.isfinite(points)) + ): + raise ValueError("a circle requires at least six finite 3-D points") + axis = _vector3(axis_xyz, name="circle axis") + axis /= np.linalg.norm(axis) + basis_x, basis_y = _plane_basis(axis) + origin = np.mean(points, axis=0) + local = points - origin + xy = np.column_stack((local @ basis_x, local @ basis_y)) + + design = np.column_stack((2.0 * xy[:, 0], 2.0 * xy[:, 1], np.ones(len(xy)))) + target = np.sum(np.square(xy), axis=1) + initial_x, initial_y, constant = np.linalg.lstsq( + design, target, rcond=None + )[0] + initial_radius = math.sqrt( + max( + float(constant + initial_x * initial_x + initial_y * initial_y), + np.finfo(float).eps, + ) + ) + + def residual(parameters: np.ndarray) -> np.ndarray: + centre = parameters[:2] + radius = float(parameters[2]) + return np.linalg.norm(xy - centre, axis=1) - radius + + fitted = least_squares( + residual, + np.asarray([initial_x, initial_y, initial_radius], dtype=float), + loss="soft_l1", + f_scale=0.0005, + max_nfev=2000, + ) + centre_xy = fitted.x[:2] + radius = abs(float(fitted.x[2])) + radial_residuals = residual( + np.asarray([centre_xy[0], centre_xy[1], radius], dtype=float) + ) + centre_xyz = origin + centre_xy[0] * basis_x + centre_xy[1] * basis_y + plane_offsets = (points - centre_xyz) @ axis + centre_xyz += float(np.median(plane_offsets)) * axis + plane_residuals = (points - centre_xyz) @ axis + return { + "axis_xyz": [float(value) for value in axis], + "center_xyz_m": [float(value) for value in centre_xyz], + "radius_m": radius, + "radial_rms_m": float( + np.sqrt(np.mean(np.square(radial_residuals))) + ), + "plane_rms_m": float( + np.sqrt(np.mean(np.square(plane_residuals))) + ), + } + + +def _project_radial( + point_xyz: Sequence[float], + circle: Mapping[str, Any], +) -> np.ndarray: + point = _vector3(point_xyz, name="trajectory point") + centre = _vector3(circle["center_xyz_m"], name="circle centre") + axis = _vector3(circle["axis_xyz"], name="circle axis") + axis /= np.linalg.norm(axis) + radial = point - centre + radial -= float(radial @ axis) * axis + if float(np.linalg.norm(radial)) < 1.0e-9: + raise ValueError("trajectory point lies on the fitted rotation axis") + return radial + + +def _signed_angle( + reference_radial_xyz: Sequence[float], + observed_radial_xyz: Sequence[float], + axis_xyz: Sequence[float], +) -> float: + reference = _vector3(reference_radial_xyz, name="reference radial") + observed = _vector3(observed_radial_xyz, name="observed radial") + axis = _vector3(axis_xyz, name="angle axis") + axis /= np.linalg.norm(axis) + reference -= float(reference @ axis) * axis + observed -= float(observed @ axis) * axis + reference /= np.linalg.norm(reference) + observed /= np.linalg.norm(observed) + return math.atan2( + float(axis @ np.cross(reference, observed)), + float(np.clip(reference @ observed, -1.0, 1.0)), + ) + + +def _reference_radial( + records: Sequence[Mapping[str, Any]], + points: Sequence[np.ndarray], + circle: Mapping[str, Any], +) -> np.ndarray: + references = [ + _project_radial(point, circle) + for record, point in zip(records, points) + if int(record.get("command_u8", -1)) == 255 + ] + if not references: + raise ValueError("trajectory is missing command-255 reference points") + radial = np.median(np.asarray(references, dtype=float), axis=0) + axis = _vector3(circle["axis_xyz"], name="circle axis") + axis /= np.linalg.norm(axis) + radial -= float(radial @ axis) * axis + if float(np.linalg.norm(radial)) < 1.0e-9: + raise ValueError("command-255 reference is degenerate") + return radial + + +def _angle_for_circle( + point_xyz: Sequence[float], + circle: Mapping[str, Any], +) -> float: + return _signed_angle( + circle["reference_radial_xyz_m"], + _project_radial(point_xyz, circle), + circle["axis_xyz"], + ) + + +def _low_command_median( + records: Sequence[Mapping[str, Any]], + values: Sequence[float], +) -> float: + selected = [ + float(value) + for record, value in zip(records, values) + if int(record.get("command_u8", 255)) <= 16 + ] + if not selected: + selected = [ + float(value) + for _, value in sorted( + zip(records, values), + key=lambda item: int(item[0].get("command_u8", 255)), + )[: max(3, len(records) // 20)] + ] + return float(np.median(selected)) + + +def _orient_circle_positive( + circle: dict[str, Any], + records: Sequence[Mapping[str, Any]], + points: Sequence[np.ndarray], +) -> dict[str, Any]: + reference = _reference_radial(records, points, circle) + circle["reference_radial_xyz_m"] = [ + float(value) for value in reference + ] + values = [_angle_for_circle(point, circle) for point in points] + if _low_command_median(records, values) < 0.0: + axis = -_vector3(circle["axis_xyz"], name="circle axis") + circle["axis_xyz"] = [float(value) for value in axis] + values = [_angle_for_circle(point, circle) for point in points] + circle["observed_arc_rad"] = float(max(values) - min(values)) + return circle + + +def _rotation_about_axis( + point_xyz: Sequence[float], + centre_xyz: Sequence[float], + axis_xyz: Sequence[float], + angle_rad: float, +) -> np.ndarray: + point = _vector3(point_xyz, name="point") + centre = _vector3(centre_xyz, name="rotation centre") + axis = _vector3(axis_xyz, name="rotation axis") + axis /= np.linalg.norm(axis) + return centre + Rotation.from_rotvec(axis * float(angle_rad)).apply( + point - centre + ) + + +def _fit_root_model( + records: Sequence[Mapping[str, Any]], +) -> dict[str, Any]: + translations = [_record_translations(record) for record in records] + point_sets = { + role: np.asarray( + [ + values[role] - values["t0"] + for values in translations + ], + dtype=float, + ) + for role in _ROOT_ROLES + } + axis, common_plane_rms = _fit_plane_axis(list(point_sets.values())) + circles: dict[str, dict[str, Any]] = {} + for role, points in point_sets.items(): + circle = _fit_circle_with_axis(points, axis) + circle["axis_xyz"] = [float(value) for value in axis] + reference = _reference_radial(records, points, circle) + circle["reference_radial_xyz_m"] = [ + float(value) for value in reference + ] + circles[role] = circle + + # Resolve the common axis sign from the consensus, then apply it to every + # circle so their signed angles are directly comparable. + consensus_before = [] + for index, record in enumerate(records): + del record + consensus_before.append( + np.median( + [ + _angle_for_circle(point_sets[role][index], circles[role]) + for role in _ROOT_ROLES + ] + ) + ) + if _low_command_median(records, consensus_before) < 0.0: + axis = -axis + for role, points in point_sets.items(): + circles[role]["axis_xyz"] = [float(value) for value in axis] + reference = _reference_radial(records, points, circles[role]) + circles[role]["reference_radial_xyz_m"] = [ + float(value) for value in reference + ] + values = [_angle_for_circle(point, circles[role]) for point in points] + circles[role]["observed_arc_rad"] = float(max(values) - min(values)) + + disagreements: list[float] = [] + for index in range(len(records)): + values = np.asarray( + [ + _angle_for_circle(point_sets[role][index], circles[role]) + for role in _ROOT_ROLES + ], + dtype=float, + ) + disagreements.append(float(np.max(np.abs(values - np.median(values))))) + + t0_points = np.asarray([values["t0"] for values in translations], dtype=float) + t0_reference = np.median(t0_points, axis=0) + anchor_drift = np.linalg.norm(t0_points - t0_reference, axis=1) + return { + "kind": "root_multi_point_circle", + "anchor_role": "t0", + "roles": list(_ROOT_ROLES), + "axis_xyz": [float(value) for value in axis], + "circles": circles, + "quality": { + "common_plane_rms_m": common_plane_rms, + "maximum_circle_plane_rms_m": max( + float(circle["plane_rms_m"]) for circle in circles.values() + ), + "maximum_circle_radial_rms_m": max( + float(circle["radial_rms_m"]) for circle in circles.values() + ), + "minimum_radius_m": min( + float(circle["radius_m"]) for circle in circles.values() + ), + "minimum_observed_arc_rad": min( + float(circle["observed_arc_rad"]) + for circle in circles.values() + ), + "p95_role_disagreement_rad": float( + np.percentile(disagreements, 95) + ), + "maximum_anchor_translation_drift_m": float( + np.max(anchor_drift) + ), + }, + } + + +def _fit_tip_model( + records: Sequence[Mapping[str, Any]], + *, + passive_ip_multiplier: float = DEFAULT_PASSIVE_IP_MULTIPLIER, +) -> dict[str, Any]: + multiplier = float(passive_ip_multiplier) + if not math.isfinite(multiplier) or multiplier <= 0.0: + raise ValueError("passive_ip_multiplier must be finite and positive") + translations = [_record_translations(record) for record in records] + t4_points = np.asarray( + [values["t4"] - values["t3"] for values in translations], + dtype=float, + ) + t5_points = np.asarray( + [values["t5"] - values["t3"] for values in translations], + dtype=float, + ) + + mcp_axis, _ = _fit_plane_axis([t4_points]) + mcp_circle = _fit_circle_with_axis(t4_points, mcp_axis) + mcp_circle = _orient_circle_positive(mcp_circle, records, t4_points) + mcp_angles = np.asarray( + [_angle_for_circle(point, mcp_circle) for point in t4_points], + dtype=float, + ) + + mcp_centre = mcp_circle["center_xyz_m"] + corrected_t5 = np.asarray( + [ + _rotation_about_axis( + point, + mcp_centre, + mcp_circle["axis_xyz"], + -angle, + ) + for point, angle in zip(t5_points, mcp_angles) + ], + dtype=float, + ) + ip_axis, _ = _fit_plane_axis([corrected_t5]) + ip_circle = _fit_circle_with_axis(corrected_t5, ip_axis) + ip_circle = _orient_circle_positive(ip_circle, records, corrected_t5) + + t3_points = np.asarray([values["t3"] for values in translations], dtype=float) + t3_reference = np.median(t3_points, axis=0) + parent_drift = np.linalg.norm(t3_points - t3_reference, axis=1) + return { + "kind": "tip_mcp_circle_with_mimic_constrained_ip", + "anchor_role": "t3", + # The G20 URDF defines thumb_ip as a passive mimic of thumb_mcp. + # A single motor-15 sweep cannot independently identify two serial + # rotations from one distal point without amplifying planar-PnP depth + # bias. Keep the T5 circle as a mechanical/vision diagnostic, but use + # the physical coupling for the runtime IP angle. + "ip_estimation_method": "mcp_mimic_constraint", + "ip_coupling_multiplier": multiplier, + "mcp_circle": mcp_circle, + "ip_circle": ip_circle, + "quality": { + "mcp_plane_rms_m": float(mcp_circle["plane_rms_m"]), + "mcp_radial_rms_m": float(mcp_circle["radial_rms_m"]), + "mcp_radius_m": float(mcp_circle["radius_m"]), + "mcp_observed_arc_rad": float(mcp_circle["observed_arc_rad"]), + "ip_plane_rms_m": float(ip_circle["plane_rms_m"]), + "ip_radial_rms_m": float(ip_circle["radial_rms_m"]), + "ip_radius_m": float(ip_circle["radius_m"]), + "ip_observed_arc_rad": float(ip_circle["observed_arc_rad"]), + "maximum_parent_translation_drift_m": float( + np.max(parent_drift) + ), + }, + } + + +def _measure_tip_angles( + tip_model: Mapping[str, Any], + values: Mapping[str, np.ndarray], +) -> tuple[float, float, float]: + """Return MCP, raw geometric IP, and constrained passive IP angles.""" + mcp_point = values["t4"] - values["t3"] + mcp_angle = _angle_for_circle(mcp_point, tip_model["mcp_circle"]) + t5_point = values["t5"] - values["t3"] + corrected_t5 = _rotation_about_axis( + t5_point, + tip_model["mcp_circle"]["center_xyz_m"], + tip_model["mcp_circle"]["axis_xyz"], + -mcp_angle, + ) + observed_ip_angle = _angle_for_circle( + corrected_t5, tip_model["ip_circle"] + ) + multiplier = tip_model.get("ip_coupling_multiplier") + constrained_ip_angle = ( + observed_ip_angle + if multiplier is None + else float(multiplier) * mcp_angle + ) + return ( + float(mcp_angle), + float(observed_ip_angle), + float(constrained_ip_angle), + ) + + +def measure_center_trajectory_angles( + models: Mapping[str, Any], + tag_translation_xyz_m: Mapping[str, Sequence[float]], +) -> dict[str, float]: + """Measure CMC/MCP and derive passive IP from one centre observation.""" + values = { + role: _vector3( + tag_translation_xyz_m[role], + name=f"{role} translation", + ) + for role in _ROLES + } + root_model = models["root"] + root_angles = [ + _angle_for_circle( + values[role] - values["t0"], + root_model["circles"][role], + ) + for role in root_model["roles"] + ] + + tip_model = models["tip"] + mcp_angle, _, ip_angle = _measure_tip_angles(tip_model, values) + return { + "thumb_cmc_pitch": float(np.median(root_angles)), + "thumb_mcp": float(mcp_angle), + "thumb_ip": float(ip_angle), + } + + +def measure_center_trajectory_phase_angles( + models: Mapping[str, Any], + tag_translation_xyz_m: Mapping[str, Sequence[float]], + *, + phase: str, +) -> dict[str, float]: + """Measure angles while compensating the active calibration phase. + + During a root sweep, T4 and T5 are carried through space by the CMC + rotation even when MCP/IP are mechanically fixed. De-rotating their + T3-relative vectors by the T3 root angle makes non-target MCP/IP drift + observable without using planar-Tag orientations. + """ + values = { + role: _vector3( + tag_translation_xyz_m[role], + name=f"{role} translation", + ) + for role in _ROLES + } + root_model = models["root"] + root_angles = { + role: _angle_for_circle( + values[role] - values["t0"], + root_model["circles"][role], + ) + for role in root_model["roles"] + } + root_t3 = float(root_angles["t3"]) + mcp_point = values["t4"] - values["t3"] + t5_point = values["t5"] - values["t3"] + if phase == PHASE_ROOT: + root_axis = _vector3( + root_model["axis_xyz"], name="root trajectory axis" + ) + root_axis /= np.linalg.norm(root_axis) + undo_root = Rotation.from_rotvec(-root_axis * root_t3) + mcp_point = undo_root.apply(mcp_point) + t5_point = undo_root.apply(t5_point) + root_angle = float(np.median(list(root_angles.values()))) + elif phase == PHASE_TIP: + # T3 is upstream of both tip joints, so it alone is the clean root + # drift witness while T4/T5 are intentionally moving. + root_angle = root_t3 + else: + raise ValueError(f"unknown calibration phase {phase}") + + tip_model = models["tip"] + phase_values = { + **values, + "t4": values["t3"] + mcp_point, + "t5": values["t3"] + t5_point, + } + mcp_angle, _, ip_angle = _measure_tip_angles( + tip_model, phase_values + ) + return { + "thumb_cmc_pitch": root_angle, + "thumb_mcp": float(mcp_angle), + "thumb_ip": float(ip_angle), + } + + +def maximum_center_non_target_drift_rad( + records: Iterable[Mapping[str, Any]], + models: Mapping[str, Any], +) -> float: + """Return non-target joint drift using centre trajectories only.""" + samples = [ + dict(record) + for record in records + if record.get("kind", "sample") == "sample" + ] + maximum = 0.0 + for phase, joint_names in ( + (PHASE_ROOT, ("thumb_mcp", "thumb_ip")), + (PHASE_TIP, ("thumb_cmc_pitch",)), + ): + phase_records = [ + record for record in samples if record.get("phase") == phase + ] + for cycle in sorted( + {int(record["cycle"]) for record in phase_records} + ): + cycle_records = [ + record + for record in phase_records + if int(record["cycle"]) == cycle + ] + reference_record = next( + ( + record + for record in cycle_records + if record.get("direction") == DIRECTION_DECREASING + and int(record.get("command_u8", -1)) == 255 + ), + None, + ) + if reference_record is None: + continue + reference = measure_center_trajectory_phase_angles( + models, + reference_record["tag_translation_xyz_m"], + phase=phase, + ) + for record in cycle_records: + measured = measure_center_trajectory_phase_angles( + models, + record["tag_translation_xyz_m"], + phase=phase, + ) + for joint_name in joint_names: + maximum = max( + maximum, + abs( + float(measured[joint_name]) + - float(reference[joint_name]) + ), + ) + return float(maximum) + + +def _fit_joint_curve( + records: Sequence[Mapping[str, Any]], + values: Sequence[float], + *, + endpoint_reference: Mapping[str, Sequence[float]] | None = None, +) -> tuple[dict[str, Any], float, float]: + by_direction: dict[str, list[list[float]]] = { + direction: [[] for _ in range(256)] for direction in DIRECTIONS + } + for record, value in zip(records, values): + direction = str(record["direction"]) + command = int(record["command_u8"]) + by_direction[direction][command].append(float(value)) + + fitted: dict[str, list[float]] = {} + maximum_correction = 0.0 + for direction in DIRECTIONS: + commands = np.asarray( + [ + command + for command, samples in enumerate(by_direction[direction]) + if samples + ], + dtype=int, + ) + if ( + commands.size < 3 + or int(commands[0]) != 0 + or int(commands[-1]) != 255 + ): + raise ValueError( + f"{direction} centre trajectory requires commands 0 and 255" + ) + raw = np.asarray( + [ + float(np.median(by_direction[direction][command])) + for command in commands + ], + dtype=float, + ) + raw -= raw[-1] + projected_samples = isotonic_nonincreasing(raw) + projected_samples -= projected_samples[-1] + maximum_correction = max( + maximum_correction, + float(np.max(np.abs(projected_samples - raw))), + ) + curve = np.interp( + np.arange(256, dtype=float), + commands.astype(float), + projected_samples, + ) + curve -= curve[255] + if endpoint_reference is not None: + curve = _regularize_coupled_zero_tail( + curve, + endpoint_reference[f"{direction}_rad"], + ) + fitted[direction] = [ + round(float(value), 8) for value in curve + ] + + decreasing = np.asarray(fitted[DIRECTION_DECREASING], dtype=float) + increasing = np.asarray(fitted[DIRECTION_INCREASING], dtype=float) + hysteresis = float(np.max(np.abs(decreasing - increasing))) + combined = 0.5 * (decreasing + increasing) + combined -= combined[255] + return ( + { + "angle_rad": [round(float(value), 8) for value in combined], + "decreasing_rad": fitted[DIRECTION_DECREASING], + "increasing_rad": fitted[DIRECTION_INCREASING], + }, + maximum_correction, + hysteresis, + ) + + +def _regularize_coupled_zero_tail( + values: Sequence[float], + reference_values: Sequence[float], + *, + maximum_tail_commands: int = 16, + zero_tolerance_rad: float = 1.0e-10, +) -> np.ndarray: + """Replace a short noise-created zero tail using a coupled joint shape. + + The passive IP and active MCP share motor 15. Close to command 255 the + IP centre trajectory is small enough that measurement noise can become + negative. Isotonic projection correctly prevents a negative angle, but + otherwise turns all remaining commands into an artificial zero plateau. + Continue the last resolved IP/MCP ratio over a short tail instead. This + changes neither the resolved part of the curve nor the exact 255 zero. + """ + curve = np.asarray(values, dtype=float).copy() + reference = np.asarray(reference_values, dtype=float) + if curve.shape != (256,) or reference.shape != (256,): + raise ValueError("endpoint curves must each contain 256 values") + if not np.all(np.isfinite(curve)) or not np.all(np.isfinite(reference)): + raise ValueError("endpoint curves must be finite") + maximum_tail = int(maximum_tail_commands) + tolerance = float(zero_tolerance_rad) + if maximum_tail < 2 or tolerance < 0.0: + raise ValueError("invalid endpoint regularization settings") + + anchor = 254 + while anchor >= 0 and abs(float(curve[anchor])) <= tolerance: + anchor -= 1 + tail_commands = 255 - anchor + if ( + anchor < 0 + or tail_commands < 2 + or tail_commands > maximum_tail + or curve[anchor] <= tolerance + or reference[anchor] <= tolerance + or abs(float(curve[255])) > tolerance + or abs(float(reference[255])) > tolerance + ): + curve[255] = 0.0 + return curve + + ratio = float(curve[anchor] / reference[anchor]) + continuation = np.maximum( + 0.0, + ratio * reference[anchor + 1 :], + ) + continuation = np.minimum.accumulate(continuation) + continuation = np.minimum(continuation, float(curve[anchor])) + continuation[-1] = 0.0 + curve[anchor + 1 :] = continuation + return curve + + +def _derive_mimic_joint_curves( + parent: Mapping[str, Sequence[float]], + *, + multiplier: float, +) -> dict[str, Any]: + """Derive a passive joint from its active parent without another fit.""" + scale = float(multiplier) + if not math.isfinite(scale) or scale <= 0.0: + raise ValueError("mimic multiplier must be finite and positive") + result: dict[str, Any] = {} + for field in ("angle_rad", "decreasing_rad", "increasing_rad"): + values = np.asarray(parent[field], dtype=float) + if values.shape != (256,) or not np.all(np.isfinite(values)): + raise ValueError(f"parent {field} must contain 256 finite values") + derived = scale * values + derived -= derived[255] + result[field] = [ + round(float(value), 8) for value in derived + ] + return result + + +def _check_trajectory_quality( + models: Mapping[str, Any], + *, + maximum_plane_rms_m: float, + maximum_radial_rms_m: float, + minimum_radius_m: float, + minimum_arc_rad: float, + maximum_root_role_disagreement_rad: float, + maximum_anchor_drift_m: float, +) -> None: + root = models["root"]["quality"] + tip = models["tip"]["quality"] + # The passive IP curve is now constrained by the G20 mechanism's URDF + # mimic relation. Its sequentially de-rotated T5 circle remains useful for + # diagnostics, but must not reject an otherwise sound CMC/MCP calibration: + # that small residual circle is exactly the view-sensitive quantity this + # constrained estimator avoids using at runtime. + plane_values = [ + float(root["common_plane_rms_m"]), + float(root["maximum_circle_plane_rms_m"]), + float(tip["mcp_plane_rms_m"]), + ] + radial_values = [ + float(root["maximum_circle_radial_rms_m"]), + float(tip["mcp_radial_rms_m"]), + ] + radius_values = [ + float(root["minimum_radius_m"]), + float(tip["mcp_radius_m"]), + ] + arc_values = [ + float(root["minimum_observed_arc_rad"]), + float(tip["mcp_observed_arc_rad"]), + ] + anchor_values = [ + float(root["maximum_anchor_translation_drift_m"]), + float(tip["maximum_parent_translation_drift_m"]), + ] + if max(plane_values) > float(maximum_plane_rms_m): + raise ValueError( + "trajectory_plane_rms_too_large:" + f"{1000.0 * max(plane_values):.2f}mm" + ) + if max(radial_values) > float(maximum_radial_rms_m): + raise ValueError( + "trajectory_circle_rms_too_large:" + f"{1000.0 * max(radial_values):.2f}mm" + ) + if min(radius_values) < float(minimum_radius_m): + raise ValueError( + "trajectory_radius_too_small:" + f"{1000.0 * min(radius_values):.2f}mm" + ) + if min(arc_values) < float(minimum_arc_rad): + raise ValueError( + "trajectory_arc_too_small:" + f"{math.degrees(min(arc_values)):.2f}deg" + ) + if ( + float(root["p95_role_disagreement_rad"]) + > float(maximum_root_role_disagreement_rad) + ): + raise ValueError( + "root_trajectory_role_disagreement_too_large:" + f"{math.degrees(float(root['p95_role_disagreement_rad'])):.2f}deg" + ) + if max(anchor_values) > float(maximum_anchor_drift_m): + raise ValueError( + "trajectory_anchor_drift_too_large:" + f"{1000.0 * max(anchor_values):.2f}mm" + ) + + +def fit_center_trajectory_curves( + records: Iterable[Mapping[str, Any]], + *, + passive_ip_multiplier: float = DEFAULT_PASSIVE_IP_MULTIPLIER, + maximum_plane_rms_m: float = 0.004, + maximum_radial_rms_m: float = 0.004, + minimum_radius_m: float = 0.005, + minimum_arc_rad: float = math.radians(15.0), + maximum_root_role_disagreement_rad: float = math.radians(5.0), + maximum_anchor_drift_m: float = 0.005, +) -> FitResult: + """Fit command curves from 3-D tag-centre trajectories. + + Runtime CMC/MCP curves, validation and non-target drift checks are produced + from centre trajectories. Passive IP follows the configured G20 mimic + relation and its T5 residual circle is diagnostic only. Planar-Tag + orientations are not required for this fit. + """ + samples = [ + dict(record) + for record in records + if record.get("kind", "sample") == "sample" + ] + root_records = [ + record for record in samples if record.get("phase") == PHASE_ROOT + ] + tip_records = [ + record for record in samples if record.get("phase") == PHASE_TIP + ] + if len(root_records) < 12 or len(tip_records) < 12: + raise ValueError("centre trajectory requires at least 12 samples per phase") + + multiplier = float(passive_ip_multiplier) + if not math.isfinite(multiplier) or multiplier <= 0.0: + raise ValueError("passive_ip_multiplier must be finite and positive") + models = { + "root": _fit_root_model(root_records), + "tip": _fit_tip_model( + tip_records, + passive_ip_multiplier=multiplier, + ), + } + _check_trajectory_quality( + models, + maximum_plane_rms_m=maximum_plane_rms_m, + maximum_radial_rms_m=maximum_radial_rms_m, + minimum_radius_m=minimum_radius_m, + minimum_arc_rad=minimum_arc_rad, + maximum_root_role_disagreement_rad=( + maximum_root_role_disagreement_rad + ), + maximum_anchor_drift_m=maximum_anchor_drift_m, + ) + + joints: dict[str, dict[str, Any]] = {} + maximum_correction = 0.0 + maximum_hysteresis = 0.0 + for joint_name, phase, motor_index in ( + ("thumb_cmc_pitch", PHASE_ROOT, 0), + ("thumb_mcp", PHASE_TIP, 15), + ): + phase_records = root_records if phase == PHASE_ROOT else tip_records + values = [ + measure_center_trajectory_angles( + models, + record["tag_translation_xyz_m"], + )[joint_name] + for record in phase_records + ] + curves, correction, hysteresis = _fit_joint_curve( + phase_records, + values, + ) + curves["motor_index"] = motor_index + joints[joint_name] = curves + maximum_correction = max(maximum_correction, correction) + maximum_hysteresis = max(maximum_hysteresis, hysteresis) + + # Motor 15 is the only independent input for both tip joints. The G20 + # kinematic model specifies thumb_ip = 1.02 * thumb_mcp. Using that + # mechanism constraint makes the passive curve identifiable and prevents + # camera-angle-dependent planar-PnP depth bias from being interpreted as + # a second physical rotation. + ip_curves = _derive_mimic_joint_curves( + joints["thumb_mcp"], + multiplier=multiplier, + ) + ip_curves["motor_index"] = 15 + ip_curves["passive"] = True + joints["thumb_ip"] = ip_curves + ip_hysteresis = float( + np.max( + np.abs( + np.asarray(ip_curves["decreasing_rad"], dtype=float) + - np.asarray(ip_curves["increasing_rad"], dtype=float) + ) + ) + ) + maximum_hysteresis = max(maximum_hysteresis, ip_hysteresis) + + # Retain the old T5 residual-circle estimate only as a diagnostic. A large + # disagreement now reports weak T5 geometry or linkage inconsistency + # without contaminating the runtime angle table. + observed_ip_values = [ + _measure_tip_angles( + models["tip"], + _record_translations(record), + )[1] + for record in tip_records + ] + observed_ip_curves, observed_correction, observed_hysteresis = ( + _fit_joint_curve( + tip_records, + observed_ip_values, + endpoint_reference=joints["thumb_mcp"], + ) + ) + mcp = np.asarray(joints["thumb_mcp"]["angle_rad"], dtype=float) + ip = np.asarray(joints["thumb_ip"]["angle_rad"], dtype=float) + observed_ip = np.asarray( + observed_ip_curves["angle_rad"], dtype=float + ) + observed_design = np.column_stack((mcp, np.ones_like(mcp))) + observed_multiplier, observed_offset = np.linalg.lstsq( + observed_design, observed_ip, rcond=None + )[0] + observed_prediction = observed_multiplier * mcp + observed_offset + observed_residual_sum = float( + np.sum(np.square(observed_ip - observed_prediction)) + ) + observed_total_sum = float( + np.sum(np.square(observed_ip - np.mean(observed_ip))) + ) + observed_r_squared = ( + 1.0 + if observed_total_sum < 1.0e-12 + else 1.0 - observed_residual_sum / observed_total_sum + ) + constrained_error = np.abs(observed_ip - ip) + models["tip"]["quality"].update( + { + "ip_estimation_method": "mcp_mimic_constraint", + "ip_coupling_multiplier": multiplier, + "ip_observed_multiplier": float(observed_multiplier), + "ip_observed_offset_rad": float(observed_offset), + "ip_observed_r_squared": float(observed_r_squared), + "ip_observed_vs_constrained_mae_rad": float( + np.mean(constrained_error) + ), + "ip_observed_vs_constrained_p95_rad": float( + np.percentile(constrained_error, 95) + ), + "ip_observed_vs_constrained_max_rad": float( + np.max(constrained_error) + ), + "ip_observed_monotonic_correction_rad": float( + observed_correction + ), + "ip_observed_hysteresis_rad": float(observed_hysteresis), + } + ) + + return FitResult( + joints=joints, + axes={}, + references={}, + ip_coupling={ + "multiplier": round(multiplier, 8), + "offset_rad": 0.0, + # Keep the acceptance metric independent: this is the R² of the + # observed T5 residual circle against MCP, not the tautological + # R² of the generated mimic curve. + "r_squared": round(float(observed_r_squared), 8), + "constrained_r_squared": 1.0, + "observed_multiplier": round(float(observed_multiplier), 8), + "observed_offset_rad": round(float(observed_offset), 8), + "observed_r_squared": round(float(observed_r_squared), 8), + }, + max_monotonic_correction_rad=maximum_correction, + max_hysteresis_rad=maximum_hysteresis, + measurement_mode="trajectory_center_3d", + trajectory_models=models, + trajectory_quality={ + "root": models["root"]["quality"], + "tip": models["tip"]["quality"], + }, + ) diff --git a/src/g20_thumb_apriltag_calibration/launch/front_thumb_calibration.launch.py b/src/g20_thumb_apriltag_calibration/launch/front_thumb_calibration.launch.py index 421903c..df9c0c5 100644 --- a/src/g20_thumb_apriltag_calibration/launch/front_thumb_calibration.launch.py +++ b/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"), diff --git a/src/g20_thumb_apriltag_calibration/test/test_acquisition.py b/src/g20_thumb_apriltag_calibration/test/test_acquisition.py index e9aaea8..ae0d484 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_acquisition.py +++ b/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) diff --git a/src/g20_thumb_apriltag_calibration/test/test_config.py b/src/g20_thumb_apriltag_calibration/test/test_config.py index b101054..76d95bc 100644 --- a/src/g20_thumb_apriltag_calibration/test/test_config.py +++ b/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 diff --git a/src/g20_thumb_apriltag_calibration/test/test_diagnostics.py b/src/g20_thumb_apriltag_calibration/test/test_diagnostics.py new file mode 100644 index 0000000..666566e --- /dev/null +++ b/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 diff --git a/src/g20_thumb_apriltag_calibration/test/test_pnp.py b/src/g20_thumb_apriltag_calibration/test/test_pnp.py new file mode 100644 index 0000000..ceaa826 --- /dev/null +++ b/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 diff --git a/src/g20_thumb_apriltag_calibration/test/test_trajectory.py b/src/g20_thumb_apriltag_calibration/test/test_trajectory.py new file mode 100644 index 0000000..aa99677 --- /dev/null +++ b/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)