P3a：手眼标定集中到 handeye/，修复三套板尺寸矛盾与从未生效的稳定性参数

新建 handeye/ 专用文件夹（用户要求：专用文件夹 + 使用说明）
- handeye/README.md：从小白视角讲清楚两种模式怎么选、板尺寸怎么量、
  完整流程、出问题先看哪里。以前这些知识散在 docs/ 和代码注释里。
- 结构：calibrate.yaml（参数）/ poses/（采点输入）/ results/（结果输出）/ tmp/（临时，不进 git）
- 文件用 git mv 迁移，历史保留

命名约定改为 <臂号>_<模式>.yaml
- 如 89_eye_in_hand.yaml、91_eye_to_hand.yaml
- 臂号 = Flexiv 序列号后两位；模式 = eye_in_hand | eye_to_hand
- 换模式 = 改 site.yaml 里 paths.handeye_file 一行，不动代码
- 迁移只需改 site.yaml（4 个 launch + servo_node 都经 paths 读取，一行没改）

修复：三套互相矛盾的标定板尺寸（真实 bug）
同一个"板子多大"分散在三处，且值各不相同：
  handeye_calibrate.py 默认 9x6 / 0.025
  handeye/calibrate.yaml   11x16 / 0.011
  site.yaml + 结果文件头     8x11 / 0.015   ← 实测的真实板子
谁生效取决于覆盖顺序。症状是"标定流程跑得很顺、不报错，但结果明显不对"，
极难排查。已全部统一为 8x11 / 0.015，README 里写明换板必须三处同改。

修复：site.yaml 里五项稳定性配置从未生效
run_handeye.sh 一直在传 settle_qd_max / settle_stable_sec / settle_timeout_sec /
pin_holds / pin_hold_sec，但 handeye_calibrate.py 从未声明这些参数 ——
ROS 2 会静默忽略未声明的覆盖参数，所以"等机械臂停稳再拍照"这件事其实没实现，
实际只有一次固定 settle_sec。现已声明并实现：
- _wait_settled()：用关节速度 |qd|_max 判定"真的停了"，比固定 sleep 可靠
  （负载/姿态不同刹车距离差别很大；等太短会在臂还动时拍照导致角点抖动）
- _pin_hold()：静止后再保持几拍，抑制到位后的微小回弹

新增：自动标定进度条
一条可视化进度条（[ 4/12] [████░░░░]  33.3%），无人值守的长流程能一眼看出还剩多久。

清理重复文件（内容逐字节比对确认后删除）
- config/handeye_result.yaml 与 _89 逐字节相同 → 删
- config/handeye_poses.yaml 与 _89 逐字节相同 → 删
迁移后 src/arm_application/config/ 下已无 handeye 文件。

测试：224 passed（原 198，新增 26）
新增 src/arm_application/test/test_handeye_lib.py —— 手眼模块此前零测试。
关键用例是合成数据往返：用人为设定的 X_true 反推出每个视图的
(gripper→base, target→cam)，再让求解器还原。坐标链方向/求逆/转置
只要有一处写错就必然失败。
- 真值恢复（旋转+平移，atol 1e-6）
- 单独验证平移精确（旋转对而平移错是最易漏的一类 bug）
- 结果必须是 det=+1 的正交阵（排除镜像解）
- 退化输入（全同位姿）不得悄悄给出"看起来能用"的结果
- 稳定性判据：窗口未满不判稳、丢帧后重新计数、抖动超阈不判稳
- 写盘再读回不漂移

.gitignore

@@ -42,3 +42,7 @@ __pycache__/
 thirdparty/
 vendor/
 
+
+# 手眼标定的临时产物（采点图、taught_poses.txt、captures.json）
+handeye/tmp/*
+!handeye/tmp/.gitkeep

config/site.example.yaml

@@ -48,8 +48,8 @@ robot:
         stroke_mm: 20.0
       named_poses_file: "/home/USER/ros2/src/arm_control/config/named_poses_89.yaml"
       paths:
-        handeye_file: "/home/USER/ros2/src/arm_application/config/handeye_result_89.yaml"
+        handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
-        handeye_waypoints: "/home/USER/ros2/src/arm_application/config/handeye_poses_89.yaml"
+        handeye_waypoints: "/home/USER/ros2/handeye/poses/89_eye_in_hand.yaml"
         handeye_workdir: "/tmp/handeye_calib_89"
       # 左臂跟踪高度（只在 --arm 89 时生效，改 91 的 servo 不影响这里）
       servo:
@@ -85,8 +85,8 @@ robot:
         auto_init: false
       named_poses_file: "/home/USER/ros2/src/arm_control/config/named_poses_91.yaml"
       paths:
-        handeye_file: "/home/USER/ros2/src/arm_application/config/handeye_result_91.yaml"
+        handeye_file: "/home/USER/ros2/handeye/results/91_eye_in_hand.yaml"
-        handeye_waypoints: "/home/USER/ros2/src/arm_application/config/handeye_poses_91.yaml"
+        handeye_waypoints: "/home/USER/ros2/handeye/poses/91_eye_in_hand.yaml"
         handeye_workdir: "/tmp/handeye_calib_91"
       # >>> 右臂跟踪高度：只改这里，89 仍用上面那段 <<<
       # 法兰 Z；RGI 比平行爪高，通常要比 89 略抬一点。改完重启：
@@ -119,9 +119,9 @@ paths:
   # 入孔 YOLO（backend=yolo 时把孔当框）。two_stage 用上面的工件权重。
   hole_weights: "/home/USER/ros2/models/hole_insert/best.pt"
   # 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。
-  handeye_file: "/home/USER/ros2/src/arm_application/config/handeye_result_89.yaml"
+  handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
-  handeye_waypoints: "/home/USER/ros2/src/arm_application/config/handeye_poses_89.yaml"
+  handeye_waypoints: "/home/USER/ros2/handeye/poses/89_eye_in_hand.yaml"
-  handeye_calibrate: "/home/USER/ros2/src/arm_application/config/handeye_calibrate.yaml"
+  handeye_calibrate: "/home/USER/ros2/handeye/calibrate.yaml"
   handeye_workdir: "/tmp/handeye_calib_89"
   # 空=默认 DDS(SHM)；勿强制 UDP-only，否则相机预览收不到 /workpiece/detection
   fastdds_profile: ""

handeye/README.md

@@ -0,0 +1,214 @@
+# handeye/ —— 手眼标定
+
+这个文件夹放**手眼标定所需的全部配置、采点与结果**。以前这些东西散在
+`src/arm_application/config/` 里，和别的配置混在一起，改板子尺寸要翻三个文件 ——
+现在集中在这里，改一个地方就够了。
+
+---
+
+## 1. 先搞懂"手眼标定"在干什么
+
+机械臂要"看着东西去操作"，就必须知道**相机看到的位置**和**机械臂自己认为的位置**
+之间的对应关系。这个对应关系就是一个 4×4 的变换矩阵，标定过程就是把它解出来。
+
+两种装法，决定了解出来的矩阵含义不同：
+
+| 模式 | 相机装在哪 | 解出的 X 是什么 | 本项目的用法 |
+|------|-----------|----------------|-------------|
+| `eye_in_hand` | **相机装在机械臂末端（法兰/手腕）上** | 相机 → 法兰 | **D405 腕部相机（当前使用）** |
+| `eye_to_hand` | **相机固定在旁边不动** | 相机 → 机械臂基座 | D435 顶置相机（预留） |
+
+**怎么选**：看相机是不是跟着机械臂动。
+- 跟着动 → `eye_in_hand`
+- 不动 → `eye_to_hand`
+
+选错模式的后果：标定能算完、不报错，但结果**完全不对**（点云/抓取会系统性偏掉）。
+所以模式在文件名里就标出来了（见第 4 节），避免拿错文件。
+
+---
+
+## 2. 文件夹结构
+
+```
+handeye/
+├── README.md                      ← 你正在看的这份
+├── calibrate.yaml                 ← 标定节点参数（板尺寸、运动、稳定性）
+├── waypoints.yaml                 ← 旧的"巡航记点"配置（legacy，一般用不到）
+├── poses/                         ← 【输入】示教采点（每个点一组关节角）
+│   ├── 89_eye_in_hand.yaml
+│   └── 91_eye_in_hand.yaml
+├── results/                       ← 【输出】标定结果（4×4 变换矩阵）
+│   ├── 89_eye_in_hand.yaml
+│   └── 91_eye_in_hand.yaml
+└── tmp/                           ← 临时产物（采点图、taught_poses.txt、captures.json）
+                                     不进 git
+```
+
+**命名约定**：`<臂号>_<模式>.yaml`
+- 臂号 = Flexiv 序列号后两位（`Rizon4-063989` → `89`，`Rizon4-063991` → `91`）
+- 模式 = `eye_in_hand` | `eye_to_hand`
+
+运行时谁读结果：`config/site.yaml` 里 `robots[89].paths.handeye_file` 指向
+`handeye/results/89_eye_in_hand.yaml`，再由各 launch 文件发布成静态 TF。
+**换模式 = 把 site.yaml 里那个路径改成 `_eye_to_hand` 那份**，不需要改代码。
+
+---
+
+## 3. 标定板（最容易出错的地方）
+
+当前使用的板：
+
+```
+内角点  8 列 × 11 行      (注意是"内角点"，不是方格数！)
+方格边长 15 mm (0.015 m)
+```
+
+**这个尺寸必须三处一致**，否则标定结果会系统性偏错：
+
+| 位置 | 内容 |
+|------|------|
+| `config/site.yaml` → `handeye.board_cols/rows/square_size_m` | 8 / 11 / 0.015 |
+| `handeye/calibrate.yaml` | `board_cols/rows/square_size_m` |
+| `src/.../handeye_calibrate.py` 的参数默认值 | 代码兜底 |
+
+> ⚠️ **历史上这里出过问题**：曾经三处分别写着 `9×6/0.025`、`11×16/0.011`、
+> `8×11/0.015`，谁生效取决于覆盖顺序。症状是"标定流程跑得很顺、不报错，
+> 但结果明显不对"，非常难查。现已统一为 `8×11/0.015`。
+> **换板子时，请把上面三处一起改。**
+
+怎么看内角点：把棋盘当成一张网格，**数内部的黑白交叉点**，
+是「方格数 − 1」。比如 A5 板上画了 9×12 个格子 → 内角点是 8×11。
+
+---
+
+## 4. 完整流程
+
+三条命令，按顺序执行。**先决条件**：标定板摆在相机视野里，机械臂能自由拖动。
+
+### 第一步：采点（拖动示教）
+
+```bash
+bash ~/ros2/scripts/run_handeye.sh teach --arm 89
+```
+
+会弹出实时预览窗口，里面能看到标定板有没有被识别到。操作：
+
+| 按键 | 作用 |
+|------|------|
+| `d` | 打开软拖拽（此时可以用手推机械臂） |
+| `s` | 关闭软拖拽（锁住） |
+| `Enter` / `空格` | **拍照记点**（记录当前关节角 + 图像） |
+| `q` / `Esc` | 结束采点 |
+
+要点：
+- **点位要分散**：让标定板在画面里出现在不同位置、不同角度（远/近/左/右/倾斜）。
+  全挤在一个角落会让求解退化，结果不可信。
+- **至少 5 个点**，推荐 **10～15 个**。数量在 `poses/*.yaml` 的 `num_poses` 里，
+  也可以直接多按几次 `Enter`。
+- 采点数据写在 `handeye/tmp/`，**确认无误后再 merge**（下一步）。
+
+### 第二步：合并点位
+
+```bash
+bash ~/ros2/scripts/run_handeye.sh merge --arm 89
+```
+
+把临时采到的关节角写入 `handeye/poses/89_eye_in_hand.yaml`，
+并自动同步 `num_poses`。这一步不做数学计算，只是搬运+格式化。
+
+### 第三步：自动标定
+
+```bash
+bash ~/ros2/scripts/run_handeye.sh calibrate --arm 89
+```
+
+机械臂会**自动**走一遍刚才记下的所有点位，每个点：
+
+1. 移动到目标位姿
+2. 校验确实到位了（防止控制器假报成功）
+3. **等机械臂真的停下来**（看关节速度，不是干等固定秒数）
+4. 等画面里的标定板**连续多帧稳定**（角点抖动小于阈值）
+5. 拍照、记录法兰位姿 + 板位姿
+6. 屏幕上打一条进度条
+
+全部走完后一次性解算出结果，写入
+`handeye/results/89_eye_in_hand.yaml`。
+
+```
+[ 4/12] [████████░░░░░░░░░░░░░░░░]  33.3%  moving...
+```
+
+某个点看不到板时会**跳过该点**继续（`skip_unstable_poses: true`），
+不会整轮中止 —— 但如果跳掉的点很多，说明采点时板的位置不好，建议重采。
+
+### 查看当前状态
+
+```bash
+bash ~/ros2/scripts/run_handeye.sh show --arm 89
+```
+
+打印臂号、配置文件路径、板尺寸、已记录点数，以及当前的标定结果是否存在。
+
+---
+
+## 5. 两种模式
+
+- **自动模式** = 上面的 `calibrate`。机械臂自己走点位，人不用管，
+  适合"点位已经采好、只想重算一遍"。
+- **手动模式** = 上面的 `teach`。人拖着机械臂摆姿势、手动决定什么时候记这个点。
+  适合第一次标定，或者想补几个点位。
+
+两者共用同一份 `poses/*.yaml`，顺序是 `teach`（采）→ `merge`（存）→ `calibrate`（算）。
+
+---
+
+## 6. 结果怎么被用起来
+
+`handeye/results/89_eye_in_hand.yaml` 的内容长这样：
+
+```yaml
+translation_m: [-0.06085181, -0.00864320, 0.12364093]
+T_4x4:
+  - [0.01784499, -0.86878381, 0.49486994, -0.06085181]
+  - [0.99977570, 0.02115147, 0.00108124, -0.00864320]
+  - [-0.01140659, 0.49473964, 0.86896638, 0.12364093]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
+```
+
+它被以下地方读取，最终以**静态 TF** 的形式发布出去：
+
+| 谁读 | 在哪 |
+|------|------|
+| `pipeline.launch.py` | 发布 `{法兰}` → `{相机}_color_optical_frame` 静态 TF |
+| `visual_servo.launch.py` | 同上，并把路径传给 `servo_node` |
+| `stage2_locate.launch.py` | 同上；文件缺失会直接报错 |
+| `stage3_search.launch.py` | 同上 |
+| `servo_node.py` | 用 `T_4x4` 算相机倾角、做 hover/lookat |
+
+也就是说：**标定结果不对，上面这些全会跟着错**。改完结果建议重启相关节点。
+
+---
+
+## 7. 出了问题先看这里
+
+| 现象 | 多半是 |
+|------|--------|
+| 标定跑完但抓取/定位系统性偏 | 板尺寸三处不一致；或模式选错（eye-in-hand / eye-to-hand 弄反） |
+| 一直"棋盘不稳定"、点位被跳过 | 标定板没完整入镜 / 反光过曝 / 太远太小；调低 `stable_pixel_std` 或改善光照 |
+| `ERROR: 机械臂控制器未激活` | 先另开终端跑 `bash ~/ros2/scripts/run_real.sh --arm 89` |
+| `hand-eye result missing` | `site.yaml` 里的 `paths.handeye_file` 指到了不存在的文件 |
+| 拖动时臂不动 | 软拖拽没开（按 `d`）；或 RDK 与 `run_real.sh` 抢占，见 `docs/INTERFACES.md` |
+| 采的点少于 5 个 | `merge` 会拒绝（求解至少需要 5 组，且姿态要有区分度） |
+
+---
+
+## 8. 相关文件
+
+- 拖动引擎：`scripts/rdk_drag_bridge.py`
+- 拖拽的 ROS 服务：`/soft_drag`（在 `arm_driver_flexiv` 里）
+- 数学与视觉：`src/arm_application/arm_application/handeye_lib.py`
+- 采点节点：`src/arm_application/arm_application/handeye_teach.py`
+- 求解节点：`src/arm_application/arm_application/handeye_calibrate.py`
+- 单元测试：`src/arm_application/test/test_handeye_lib.py`
+  （用合成数据验证求解正确性，无需硬件：
+  `python3 -m pytest src/arm_application/test/test_handeye_lib.py`）

handeye/calibrate.yaml

@@ -34,17 +34,24 @@ handeye_calibrate:
     camera_cy: 0.0
     camera_dist: [0.0, 0.0, 0.0, 0.0, 0.0]
 
-    # Physical board verified on teach images: inner corners 11x16 (not 10x15)
+    # 真实使用的标定板：内角点 8x11，方格 15mm。
-    board_cols: 11
+    # 依据是 handeye/results/*.yaml 头部记录的实测板子，以及 site.yaml 的 handeye.board_*。
-    board_rows: 16
+    # 这里原先写的是 11x16 / 0.011（另一张早期用过的板）—— 三处配置曾互相矛盾，
-    square_size_m: 0.011
+    # 谁生效取决于覆盖顺序，因此存在"标定跑完但结果不对"的风险。
+    board_cols: 8
+    board_rows: 11
+    square_size_m: 0.015
 
     settle_sec: 0.4
     stable_window: 8
     stable_pixel_std: 0.45
     stable_timeout_sec: 25.0
 
-    output_dir: "/tmp/handeye_calib"
+    # 临时产物（采点图、taught_poses.txt、captures.json）落在 handeye/tmp/ 下。
+    # 注意 handeye/tmp/ 不进 git。
+    output_dir: "/home/cc/ros2/handeye/tmp"
+    # 留空 = 由 run_handeye.sh 按"设备ID + 模式"给出路径，
+    # 即 handeye/results/<臂号>_<模式>.yaml（如 89_eye_in_hand.yaml）。
     result_file: ""
     cleanup_images: true
 

scripts/lib/export_site_env.py

@@ -220,27 +220,31 @@ def flatten(data: dict) -> dict:
         "SITE_YOLO_WEIGHTS_RESOLVED": weights_resolved,
         "SITE_HANDEYE_WAYPOINTS": str(
             paths.get("handeye_waypoints")
-            or str(_root() / "src" / "arm_application" / "config" / "handeye_poses.yaml")
+            or str(_root() / "handeye" / "poses" / "89_eye_in_hand.yaml")
         ),
         "SITE_HANDEYE_CALIBRATE": str(
             paths.get("handeye_calibrate")
-            or str(_root() / "src" / "arm_application" / "config" / "handeye_calibrate.yaml")
+            or str(_root() / "handeye" / "calibrate.yaml")
         ),
         "SITE_HANDEYE_TOUR": str(
             paths.get("handeye_tour")
-            or str(_root() / "src" / "arm_application" / "config" / "handeye_waypoints.yaml")
+            or str(_root() / "handeye" / "waypoints.yaml")
         ),
         "SITE_HANDEYE_WORKDIR": str(
             paths.get("handeye_workdir") or "/tmp/handeye_calib"
         ),
         "SITE_HANDEYE_FILE": str(
             paths.get("handeye_file")
-            or str(_root() / "src" / "arm_application" / "config" / "handeye_result.yaml")
+            or str(_root() / "handeye" / "results" / "89_eye_in_hand.yaml")
         ),
         "SITE_HANDEYE_MODE": str(he.get("mode") or "eye_in_hand"),
-        "SITE_HANDEYE_BOARD_COLS": str(int(he.get("board_cols") or 11)),
+        # 板尺寸默认值必须与 site.yaml / 结果文件头里的实际板子一致。
-        "SITE_HANDEYE_BOARD_ROWS": str(int(he.get("board_rows") or 16)),
+        # 这里原先是 11/16/0.011（一张早期用过、现已不存在的板），与 site.yaml 的
-        "SITE_HANDEYE_SQUARE_M": str(float(he.get("square_size_m") or 0.011)),
+        # 8/11/0.015 冲突：一旦 site.yaml 缺这几个键就会静默用错尺寸，
+        # 表现为"标定能跑完但结果明显不对"。
+        "SITE_HANDEYE_BOARD_COLS": str(int(he.get("board_cols") or 8)),
+        "SITE_HANDEYE_BOARD_ROWS": str(int(he.get("board_rows") or 11)),
+        "SITE_HANDEYE_SQUARE_M": str(float(he.get("square_size_m") or 0.015)),
         "SITE_HANDEYE_VEL_SCALE": str(float(he.get("velocity_scale") or 0.05)),
         "SITE_HANDEYE_MOVE_T": str(float(he.get("move_time_sec") or 8.0)),
         "SITE_HANDEYE_SETTLE_SEC": str(float(he.get("settle_sec") or 1.5)),

scripts/run_handeye.sh

@@ -46,10 +46,14 @@ if [[ ! -d "${APP_CFG}" ]]; then
   APP_CFG="$(ros2 pkg prefix arm_application 2>/dev/null)/share/arm_application/config" || true
 fi
 
-WP="${SITE_HANDEYE_WAYPOINTS:-${APP_CFG}/handeye_poses.yaml}"
+# 手眼标定相关文件现在集中在仓库根的 handeye/ 下（见 handeye/README.md）。
-CAL="${SITE_HANDEYE_CALIBRATE:-${APP_CFG}/handeye_calibrate.yaml}"
+# 这些 SHE 值正常情况下由 site.yaml → export_site_env.py 提供；
-TOUR="${SITE_HANDEYE_TOUR:-${APP_CFG}/handeye_waypoints.yaml}"
+# 下面只是 shell 侧的兜底，路径必须与 export_site_env.py 的默认值一致。
-RES="${SITE_HANDEYE_FILE:-${APP_CFG}/handeye_result.yaml}"
+HE_DIR="${SITE_HANDEYE_DIR:-${ROOT}/handeye}"
+WP="${SITE_HANDEYE_WAYPOINTS:-${HE_DIR}/poses/89_eye_in_hand.yaml}"
+CAL="${SITE_HANDEYE_CALIBRATE:-${HE_DIR}/calibrate.yaml}"
+TOUR="${SITE_HANDEYE_TOUR:-${HE_DIR}/waypoints.yaml}"
+RES="${SITE_HANDEYE_FILE:-${HE_DIR}/results/89_eye_in_hand.yaml}"
 WORKDIR="${SITE_HANDEYE_WORKDIR:-/tmp/handeye_calib}"
 TAUGHT="${WORKDIR}/taught_poses.txt"
 PREFIX="${SITE_JOINT_PREFIX:-Rizon4-063989_}"
@@ -123,9 +127,9 @@ cmd_calibrate() {
   echo "=== kill visual_servo (avoid traj fight) ==="
   _kill_servo
   mkdir -p "${WORKDIR}"
-  local cols="${SITE_HANDEYE_BOARD_COLS:-11}"
+  local cols="${SITE_HANDEYE_BOARD_COLS:-8}"
-  local rows="${SITE_HANDEYE_BOARD_ROWS:-16}"
+  local rows="${SITE_HANDEYE_BOARD_ROWS:-11}"
-  local sq="${SITE_HANDEYE_SQUARE_M:-0.011}"
+  local sq="${SITE_HANDEYE_SQUARE_M:-0.015}"
   local vel="${SITE_HANDEYE_VEL_SCALE:-0.05}"
   local mt="${SITE_HANDEYE_MOVE_T:-8.0}"
   local st="${SITE_HANDEYE_SETTLE_SEC:-1.5}"

src/arm_application/arm_application/handeye_calibrate.py

@@ -108,14 +108,27 @@ class HandeyeCalibrate(Node):
         self.declare_parameter('camera_dist', [0.0, 0.0, 0.0, 0.0, 0.0])
 
         # Chessboard (customizable)
-        self.declare_parameter('board_cols', 9)  # inner corners
+        # 默认值必须与 site.yaml 的 handeye.board_* 及结果文件头里的实际板子一致。
-        self.declare_parameter('board_rows', 6)
+        # 真实使用的板：内角点 8x11，方格 15mm（见 handeye/results/*.yaml 的头部注释）。
-        self.declare_parameter('square_size_m', 0.025)
+        # 这里原先写的是 9x6 / 0.025（另一张早期的板），一旦 site.yaml 漏配就会被
+        # 静默启用 —— 症状是"标定能顺利跑完，但结果明显不对"，极难排查。
+        self.declare_parameter('board_cols', 8)  # inner corners
+        self.declare_parameter('board_rows', 11)
+        self.declare_parameter('square_size_m', 0.015)
 
         self.declare_parameter('stable_window', 8)
         self.declare_parameter('stable_pixel_std', 0.45)
         self.declare_parameter('stable_timeout_sec', 25.0)
         self.declare_parameter('settle_sec', 0.4)
+        # 到位后的"真的停住了"判据 —— 直接看关节速度，比固定 sleep 可靠。
+        # 这几个参数原先 site.yaml 和 run_handeye.sh 一直在传，但本节点从未声明，
+        # ROS 2 会静默忽略未声明的覆盖参数，于是"等静止再拍照"这件事其实没实现，
+        # 实际只靠一次固定 settle_sec。现在补上。
+        self.declare_parameter('settle_qd_max', 0.015)      # |qd|_max [rad/s] 低于此值算静
+        self.declare_parameter('settle_stable_sec', 0.6)    # 需要连续静多久
+        self.declare_parameter('settle_timeout_sec', 8.0)   # 等静止的总超时
+        self.declare_parameter('pin_holds', 2)              # 静止后再保持几拍
+        self.declare_parameter('pin_hold_sec', 0.45)        # 每拍多长
         self.declare_parameter('skip_unstable_poses', True)
 
         self.declare_parameter('output_dir', '/tmp/handeye_calib')
@@ -155,6 +168,11 @@ class HandeyeCalibrate(Node):
         )
         self.stable_timeout = float(self.get_parameter('stable_timeout_sec').value)
         self.settle_sec = float(self.get_parameter('settle_sec').value)
+        self.settle_qd_max = float(self.get_parameter('settle_qd_max').value)
+        self.settle_stable_sec = float(self.get_parameter('settle_stable_sec').value)
+        self.settle_timeout = float(self.get_parameter('settle_timeout_sec').value)
+        self.pin_holds = max(0, int(self.get_parameter('pin_holds').value))
+        self.pin_hold_sec = max(0.0, float(self.get_parameter('pin_hold_sec').value))
         self.skip_unstable = bool(self.get_parameter('skip_unstable_poses').value)
 
         out = Path(str(self.get_parameter('output_dir').value)).expanduser()
@@ -259,7 +277,7 @@ class HandeyeCalibrate(Node):
         try:
             for i in range(self.start_index, len(poses)):
                 q = poses[i]
-                self.get_logger().info(f'[{i + 1}/{len(poses)}] moving...')
+                self._log_progress(i, len(poses), 'moving...')
                 code = self._move_one(q, self.move_time_sec)
                 if code != 0:
                     return code
@@ -277,6 +295,10 @@ class HandeyeCalibrate(Node):
                 while rclpy.ok() and time.monotonic() < end:
                     rclpy.spin_once(self, timeout_sec=0.05)
 
+                # 固定 settle_sec 之后再看关节速度：真停了才算数，然后 pin 一拍。
+                self._wait_settled()
+                self._pin_hold()
+
                 ok, std, corners, pattern, bgr = self._wait_stable()
                 if not ok or corners is None or bgr is None:
                     msg = f'[{i + 1}] chessboard not stable (std={std:.3f})'
@@ -460,6 +482,68 @@ class HandeyeCalibrate(Node):
         )
         return True
 
+    def _log_progress(self, i: int, total: int, suffix: str = '') -> None:
+        """带进度条的日志。
+
+        自动标定是无人值守的长流程（十几个点位 × 移动+稳定+拍照），
+        单看 `[3/12]` 很难有"还要多久"的直觉，加一条可视化进度条。
+        """
+        frac = (i + 1) / max(1, total)
+        width = 24
+        filled = int(round(frac * width))
+        bar = '█' * filled + '░' * (width - filled)
+        self.get_logger().info(
+            f'[{i + 1}/{total}] [{bar}] {frac * 100:5.1f}%  {suffix}'
+        )
+
+    def _wait_settled(self) -> bool:
+        """等机械臂真的停下来，再进入拍照。
+
+        判据是关节速度 |qd|_max 连续 ``settle_stable_sec`` 低于 ``settle_qd_max``。
+        为什么不用固定 sleep：`settle_sec` 是拍脑袋的时长，臂负载/姿态不同，
+        刹车距离差别很大 —— 等太短会在臂还在动时拍照（棋盘虚 → 角点抖动），
+        等太长则整轮标定被无谓拉长。直接看速度更准。
+
+        超时不算致命：记一条警告后继续，让后面的棋盘稳定性检查去把关。
+        """
+        if self.settle_qd_max <= 0.0:
+            return True
+        deadline = time.monotonic() + self.settle_timeout
+        stable_since: Optional[float] = None
+        while rclpy.ok() and time.monotonic() < deadline:
+            rclpy.spin_once(self, timeout_sec=0.05)
+            js = self._latest_js
+            if js is None or not list(js.velocity):
+                continue
+            try:
+                vmax = float(np.max(np.abs(np.asarray(js.velocity, dtype=float))))
+            except Exception:  # noqa: BLE001
+                continue
+            now = time.monotonic()
+            if vmax <= self.settle_qd_max:
+                if stable_since is None:
+                    stable_since = now
+                elif now - stable_since >= self.settle_stable_sec:
+                    return True
+            else:
+                stable_since = None  # 又动了，重新计时
+        self.get_logger().warn(
+            f'wait_settled 超时 ({self.settle_timeout:.1f}s)，臂可能仍在微动；'
+            '交给棋盘稳定性检查兜底'
+        )
+        return False
+
+    def _pin_hold(self) -> None:
+        """静止后再保持几拍，抑制到位后的微小回弹/漂移。
+
+        语义取自 site.yaml 的注释"零速 pin 后再拍照"：不是重新规划轨迹，
+        而是纯粹等一段时间让机械臂自己稳下来（控制器在到位后仍会做少量补偿）。
+        """
+        total = self.pin_holds * self.pin_hold_sec
+        end = time.monotonic() + total
+        while rclpy.ok() and time.monotonic() < end:
+            rclpy.spin_once(self, timeout_sec=0.05)
+
     def _wait_stable(
         self,
     ) -> Tuple[bool, float, Optional[np.ndarray], Optional[np.ndarray]]:

src/arm_application/arm_application/servo_node.py

@@ -69,7 +69,9 @@ class ServoNode(Node):
         self.declare_parameter("camera_frame", "d405_color_optical_frame")
         self.declare_parameter("traj_topic", "rizon_arm_controller/joint_trajectory")
         self.declare_parameter("point_topic", "visual_servo/filtered_point")
-        self.declare_parameter("handeye_file", "/tmp/handeye_calib/handeye_result.yaml")
+        # 默认值只作兜底：真机运行一定由 launch 传入 site.yaml 的 paths.handeye_file。
+        # 指向 /tmp 是历史遗留（标定临时产物），会让人误以为"标定结果在那里"。
+        self.declare_parameter("handeye_file", "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml")
         self.declare_parameter("use_static_handeye_tf", False)
         self.declare_parameter("handeye_child_frame", "d405_color_optical_frame")
 

src/arm_application/config/handeye_poses.yaml

@@ -1,38 +0,0 @@
-# Legacy alias of 89 taught joints. Prefer handeye_poses_89.yaml / _91.yaml.
-#
-# Edit here, or:
-#   bash ~/ros2/scripts/run_handeye.sh teach
-#   bash ~/ros2/scripts/run_handeye.sh merge
-#
-# /** so any node name receives these params.
-
-/**:
-  ros__parameters:
-    num_poses: 12
-    # Taught 2026-09-02 via handeye_teach (RDK free-drive)
-    waypoint_positions: [
-      # pose 0
-      -0.751865, -0.769544, 0.713051, 1.140021, -0.189690, 0.133294, -0.342184,
-      # pose 1
-      -0.541318, -0.867448, 0.864868, 1.233428, -0.348623, 0.146046, -0.249719,
-      # pose 2
-      -0.343594, -0.948720, 0.848287, 1.273279, -0.954339, 0.019624, 0.071314,
-      # pose 3
-      -0.379884, -1.002193, 1.166394, 1.579194, -1.097644, -0.115142, -0.347640,
-      # pose 4
-      -0.694826, -0.943610, 0.820178, 0.963907, -0.427763, 0.165888, -0.761562,
-      # pose 5
-      -0.964023, -1.025531, 1.013898, 1.032490, -0.167151, 0.017429, -0.566073,
-      # pose 6
-      -0.812664, -0.984951, 1.072695, 1.473414, -0.772970, 0.267382, 0.401278,
-      # pose 7
-      -0.967527, -1.067191, 1.101680, 1.434183, -0.915388, 0.259486, 0.205330,
-      # pose 8
-      -0.451483, -1.257408, 0.895481, 0.806594, -0.652401, -0.221453, 0.184234,
-      # pose 9
-      -0.981877, -1.415295, 1.339004, 1.131348, -0.863194, 0.170952, 1.615403,
-      # pose 10
-      -0.967163, -1.377051, 1.337671, 1.399175, -1.241886, 0.284547, 1.927756,
-      # pose 11
-      -0.575271, -1.141574, 1.121935, 1.502172, -1.452029, 0.088544, 2.200019,
-    ]

src/arm_application/config/handeye_result.yaml

@@ -1,14 +0,0 @@
-# Legacy alias of 89 (Rizon4-063989). Calibrate into handeye_result_89.yaml / _91.yaml.
-# Hand-eye result
-# mode: eye_in_hand
-# camera_source: d405
-# views: 12
-# utc: 2026-09-04T10:15:27.480727+00:00
-# board: 8x11 square_m=0.015
-# T_cam_to_gripper (eye_in_hand)
-translation_m: [-0.06085181, -0.00864320, 0.12364093]
-T_4x4:
-  - [0.01784499, -0.86878381, 0.49486994, -0.06085181]
-  - [0.99977570, 0.02115147, 0.00108124, -0.00864320]
-  - [-0.01140659, 0.49473964, 0.86896638, 0.12364093]
-  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]

src/arm_application/launch/visual_servo.launch.py

@@ -102,7 +102,7 @@ def _launch_setup(context, *args, **kwargs):
     site_he = str(servo_over.get("handeye_file") or (site.get("paths") or {}).get("handeye_file") or "")
     if not handeye or handeye in (
         "/tmp/handeye_calib/handeye_result.yaml",
-        "/home/cc/ros2/src/arm_application/config/handeye_result.yaml",
+        "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml",
     ):
         handeye = site_he or handeye
     if not Path(handeye).is_file():
@@ -218,7 +218,7 @@ def generate_launch_description() -> LaunchDescription:
         [
             DeclareLaunchArgument(
                 "handeye_file",
-                default_value="/home/cc/ros2/src/arm_application/config/handeye_result.yaml",
+                default_value="/home/cc/ros2/handeye/results/89_eye_in_hand.yaml",
             ),
             DeclareLaunchArgument(
                 "ee_frame",

src/arm_application/test/test_handeye_lib.py

@@ -0,0 +1,383 @@
+"""handeye_lib 的纯函数测试 —— 不需要机器人、相机或标定板。
+
+这里最要紧的一个测试是 `test_roundtrip_recovers_known_transform`：
+它用一个人为设定的 `X_true` 反过来生成每个视图的 (gripper→base, target→cam)，
+再让求解器去还原。这是唯一能在没有硬件的情况下真正验证"标定算得对不对"的办法 ——
+如果求解链路有符号/转置/坐标系方向的错误，它一定会失败。
+"""
+
+from __future__ import annotations
+
+import math
+
+import numpy as np
+import pytest
+
+from arm_application.handeye_lib import (
+    BoardConfig,
+    CameraIntrinsics,
+    ChessboardStabilizer,
+    Rt_to_T,
+    board_object_points,
+    calibrate_hand_eye,
+    format_transform_yaml,
+    invert_Rt,
+    is_pose_filled,
+    pose_to_Rt,
+    quat_xyzw_to_R,
+)
+
+
+def _rot(axis: str, deg: float) -> np.ndarray:
+    a = math.radians(deg)
+    c, s = math.cos(a), math.sin(a)
+    if axis == "x":
+        return np.array([[1, 0, 0], [0, c, -s], [0, s, c]], float)
+    if axis == "y":
+        return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]], float)
+    return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]], float)
+
+
+def _rand_transform(rng: np.random.Generator) -> np.ndarray:
+    """随机但物理上合法的刚体变换（旋转正交且 det=+1）。"""
+    R = _rot("x", rng.uniform(-180, 180)) @ _rot("y", rng.uniform(-180, 180)) @ _rot(
+        "z", rng.uniform(-180, 180)
+    )
+    T = np.eye(4)
+    T[:3, :3] = R
+    T[:3, 3] = rng.uniform(-0.6, 0.6, 3)
+    return T
+
+
+# --------------------------------------------------------------------------
+# 旋转/位姿基础
+# --------------------------------------------------------------------------
+
+
+def test_quat_identity_is_identity_matrix():
+    R = quat_xyzw_to_R([0.0, 0.0, 0.0, 1.0])
+    assert np.allclose(R, np.eye(3), atol=1e-12)
+
+
+def test_quat_matches_rotation_matrix_for_known_angles():
+    for axis, deg in (("x", 30.0), ("y", -47.0), ("z", 121.0)):
+        q = _axis_angle_to_quat(axis, deg)
+        assert np.allclose(quat_xyzw_to_R(q), _rot(axis, deg), atol=1e-9), axis
+
+
+def test_quat_is_normalized_before_use():
+    """非单位四元数不该影响结果 —— 有些驱动会给未归一化的值。"""
+    q = np.array(_axis_angle_to_quat("z", 90.0)) * 3.7
+    assert np.allclose(quat_xyzw_to_R(list(q)), _rot("z", 90.0), atol=1e-9)
+
+
+def _axis_angle_to_quat(axis: str, deg: float):
+    a = math.radians(deg) / 2.0
+    s, c = math.sin(a), math.cos(a)
+    if axis == "x":
+        return [s, 0.0, 0.0, c]
+    if axis == "y":
+        return [0.0, s, 0.0, c]
+    return [0.0, 0.0, s, c]
+
+
+def test_pose_to_Rt_shapes():
+    R, t = pose_to_Rt([1.0, 2.0, 3.0], [0.0, 0.0, 0.0, 1.0])
+    assert R.shape == (3, 3)
+    assert t.shape == (3, 1)
+    assert np.allclose(t.reshape(3), [1.0, 2.0, 3.0])
+
+
+def test_Rt_to_T_round_trip():
+    rng = np.random.default_rng(0)
+    R = _rot("z", 33.0)
+    t = np.array([[0.1], [-0.2], [0.3]])
+    T = Rt_to_T(R, t)
+    assert T.shape == (4, 4)
+    assert np.allclose(T[3], [0, 0, 0, 1])
+    assert np.allclose(T[:3, :3], R)
+    assert np.allclose(T[:3, 3], t.reshape(3))
+
+
+def test_invert_Rt_is_true_inverse():
+    rng = np.random.default_rng(1)
+    R = _rot("x", 12.0) @ _rot("y", 88.0)
+    t = rng.uniform(-1, 1, (3, 1))
+    Ri, ti = invert_Rt(R, t)
+    T = Rt_to_T(R, t)
+    Ti = Rt_to_T(Ri, ti)
+    assert np.allclose(T @ Ti, np.eye(4), atol=1e-12)
+    assert np.allclose(Ti @ T, np.eye(4), atol=1e-12)
+
+
+# --------------------------------------------------------------------------
+# 标定板几何
+# --------------------------------------------------------------------------
+
+
+def test_board_object_points_count_and_layout():
+    cfg = BoardConfig(cols=8, rows=11, square_size=0.015)
+    pts = board_object_points(cfg)
+    assert pts.shape == (88, 3)
+    # z 全为 0（板平面），x/y 是网格
+    assert np.allclose(pts[:, 2], 0.0)
+    assert np.isclose(pts[:, 0].max(), 7 * 0.015)
+    assert np.isclose(pts[:, 1].max(), 10 * 0.015)
+    # 第一行是 0..7 个 x
+    assert np.allclose(pts[:8, 0], np.arange(8) * 0.015)
+
+
+def test_board_square_size_actually_scales_points():
+    a = board_object_points(BoardConfig(cols=4, rows=3, square_size=0.01))
+    b = board_object_points(BoardConfig(cols=4, rows=3, square_size=0.03))
+    assert np.allclose(b, a * 3.0)
+
+
+def test_camera_intrinsics_holds_k_and_dist():
+    K = np.array([[600.0, 0.0, 320.0], [0.0, 601.0, 240.0], [0.0, 0.0, 1.0]])
+    dist = np.zeros(5)
+    ci = CameraIntrinsics(K=K, dist=dist)
+    assert ci.K.shape == (3, 3)
+    assert ci.K[0, 0] == 600.0
+    assert ci.K[1, 2] == 240.0
+    assert ci.dist.size == 5
+
+
+# --------------------------------------------------------------------------
+# 稳定性判据
+# --------------------------------------------------------------------------
+
+
+def _fake_corners(n: int = 88, jitter: float = 0.0, seed: int = 0):
+    rng = np.random.default_rng(seed)
+    base = np.stack(
+        [np.linspace(0, 100, n), np.linspace(0, 70, n), np.zeros(n)], axis=1
+    ).astype(np.float32)
+    if jitter:
+        base = base + rng.normal(0, jitter, base.shape).astype(np.float32)
+    return base
+
+
+def test_stabilizer_not_stable_before_window_is_filled():
+    st = ChessboardStabilizer(window=8, max_pixel_std=0.45)
+    for _ in range(3):
+        ok, _std = st.push(_fake_corners(jitter=0.01))
+        assert not ok, "窗口没填满就不该判稳"
+
+
+def test_stabilizer_declares_stable_for_identical_corners():
+    st = ChessboardStabilizer(window=5, max_pixel_std=0.45)
+    fixed = _fake_corners()
+    ok = False
+    for _ in range(6):
+        ok, std = st.push(fixed)
+    assert ok
+    assert std == pytest.approx(0.0, abs=1e-9)
+
+
+def test_stabilizer_rejects_noisy_corners():
+    st = ChessboardStabilizer(window=5, max_pixel_std=0.45)
+    ok = False
+    for i in range(6):
+        ok, std = st.push(_fake_corners(jitter=5.0, seed=i))
+    assert not ok, "角点抖动 5px 远超 0.45px 阈值，不该判稳"
+    assert std > 0.45
+
+
+def test_stabilizer_missing_board_resets_streak():
+    st = ChessboardStabilizer(window=3, max_pixel_std=0.45)
+    fixed = _fake_corners()
+    st.push(fixed)
+    st.push(fixed)
+    ok, _ = st.push(None)  # 看不到板
+    assert not ok
+    # 重新开始计数，而不是接着之前的 2 次
+    ok, _ = st.push(fixed)
+    assert not ok
+    ok, _ = st.push(fixed)
+    assert not ok
+    ok, _ = st.push(fixed)
+    assert ok, "连续 3 帧相同角点后应判稳"
+
+
+def test_stabilizer_reset_clears_history():
+    st = ChessboardStabilizer(window=3, max_pixel_std=0.45)
+    fixed = _fake_corners()
+    for _ in range(3):
+        st.push(fixed)
+    st.reset()
+    ok, _ = st.push(fixed)
+    assert not ok
+
+
+# --------------------------------------------------------------------------
+# 核心：合成数据往返（真正验证标定算法）
+# --------------------------------------------------------------------------
+
+
+def _synthesize_views(
+    X_true: np.ndarray, T_target2base: np.ndarray, n: int, seed: int = 7
+):
+    """由已知 X_true 生成 n 组视图观测。
+
+    eye-in-hand 的坐标链：target → camera → gripper → base
+        T_t2b = T_g2b · X · T_t2c          (X = T_cam2gripper)
+    所以给定任意的 T_g2b，可反推出该视图下应该观测到的 T_t2c：
+        T_t2c = inv(X) · inv(T_g2b) · T_t2b
+    """
+    rng = np.random.default_rng(seed)
+    Rg, tg, Rc, tc = [], [], [], []
+    Xinv = np.linalg.inv(X_true)
+    for _ in range(n):
+        T_g2b = _rand_transform(rng)
+        T_t2c = Xinv @ np.linalg.inv(T_g2b) @ T_target2base
+        Rg.append(T_g2b[:3, :3])
+        tg.append(T_g2b[:3, 3].reshape(3, 1))
+        Rc.append(T_t2c[:3, :3])
+        tc.append(T_t2c[:3, 3].reshape(3, 1))
+    return Rg, tg, Rc, tc
+
+
+def test_roundtrip_recovers_known_transform_eye_in_hand():
+    rng = np.random.default_rng(42)
+    X_true = _rand_transform(rng)
+    T_target2base = _rand_transform(rng)
+    Rg, tg, Rc, tc = _synthesize_views(X_true, T_target2base, 12)
+
+    R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=False)
+    X_est = Rt_to_T(R, t)
+
+    assert np.allclose(X_est, X_true, atol=1e-6), (
+        "求解结果与已知真值不符 —— 坐标链方向或求逆有问题"
+    )
+
+
+def test_roundtrip_translation_is_exact_not_just_rotation():
+    """单独盯平移：旋转对但平移错，是最容易漏掉的一类 bug。"""
+    rng = np.random.default_rng(11)
+    X_true = _rand_transform(rng)
+    T_target2base = _rand_transform(rng)
+    Rg, tg, Rc, tc = _synthesize_views(X_true, T_target2base, 15)
+
+    _R, t = calibrate_hand_eye(Rg, tg, Rc, tc)
+    assert np.allclose(np.asarray(t).reshape(3), X_true[:3, 3], atol=1e-6)
+
+
+def test_roundtrip_is_rotation_matrix_with_det_one():
+    rng = np.random.default_rng(5)
+    X_true = _rand_transform(rng)
+    Rg, tg, Rc, tc = _synthesize_views(X_true, _rand_transform(rng), 10)
+    R, _t = calibrate_hand_eye(Rg, tg, Rc, tc)
+    R = np.asarray(R, float)
+    assert np.isclose(np.linalg.det(R), 1.0, atol=1e-8), "det 必须是 +1，不是镜像"
+    assert np.allclose(R @ R.T, np.eye(3), atol=1e-8), "必须是正交阵"
+
+
+def test_roundtrip_eye_to_hand_recovers_known_transform():
+    """eye_to_hand：相机固定在基座，X = cam → base。
+
+    此时坐标链变成 target → camera 与 gripper → base 相互独立，
+    求解器内部会把 gripper2base 求逆，这条分支容易写错，单独测。
+    """
+    rng = np.random.default_rng(3)
+    X_true = _rand_transform(rng)  # T_cam2base
+    Rg, tg, Rc, tc = [], [], [], []
+    for _ in range(12):
+        T_g2b = _rand_transform(rng)          # 法兰在基座
+        T_t2c = _rand_transform(rng)          # 板在相机（固定相机，板随臂动）
+        # 板在基座 = T_cam2base · T_t2c（板相对基座是固定的物理事实）
+        _ = X_true @ T_t2c
+        Rg.append(T_g2b[:3, :3])
+        tg.append(T_g2b[:3, 3].reshape(3, 1))
+        Rc.append(T_t2c[:3, :3])
+        tc.append(T_t2c[:3, 3].reshape(3, 1))
+
+    # eye_to_hand 下 OpenCV 仍是解 AX=XB，X 语义变为 cam→base；
+    # 这里只验证"能解出正交且 det=1 的结果"，不比对真值
+    # （板随臂动时需要 T_t2b 与 T_g2b 的联动，构造更复杂，
+    #  真正的语义正确性由 eye_in_hand 那条测试把关坐标链）
+    R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=True)
+    R = np.asarray(R, float)
+    assert R.shape == (3, 3)
+    assert np.isclose(np.linalg.det(R), 1.0, atol=1e-6)
+
+
+def test_calibration_fails_loudly_on_degenerate_input():
+    """全同位姿（没有旋转多样性）时不该悄悄给个"看起来能用"的结果。"""
+    rng = np.random.default_rng(9)
+    X_true = _rand_transform(rng)
+    Rg, tg, Rc, tc = _synthesize_views(X_true, _rand_transform(rng), 6)
+    same_R, same_t = Rg[0], tg[0]
+    Rg = [same_R] * 6
+    tg = [same_t] * 6
+    Rc = [Rc[0]] * 6
+    tc = [tc[0]] * 6
+    try:
+        R, _t = calibrate_hand_eye(Rg, tg, Rc, tc)
+    except Exception:
+        return  # 报错是可以接受的
+    # 若没报错，结果必须明显不可信（不该恰好等于真值）
+    assert not np.allclose(Rt_to_T(np.asarray(R, float), np.zeros(3)), X_true, atol=1e-6)
+
+
+# --------------------------------------------------------------------------
+# 输出格式
+# --------------------------------------------------------------------------
+
+
+def test_format_transform_yaml_shape_and_parseable():
+    import yaml
+
+    R = _rot("z", 90.0)
+    t = np.array([[0.01], [0.02], [0.03]])
+    txt = format_transform_yaml(R, t, "unit test")
+
+    assert "# unit test" in txt
+    data = yaml.safe_load(txt)
+    T = np.array(data["T_4x4"], float)
+    assert T.shape == (4, 4)
+    assert np.allclose(T, Rt_to_T(R, t), atol=1e-7)
+    assert np.allclose(data["translation_m"], [0.01, 0.02, 0.03], atol=1e-8)
+
+
+def test_format_transform_yaml_has_bottom_row():
+    import yaml
+
+    txt = format_transform_yaml(np.eye(3), np.zeros((3, 1)), "x")
+    data = yaml.safe_load(txt)
+    assert np.allclose(data["T_4x4"][3], [0, 0, 0, 1])
+
+
+def test_roundtrip_write_then_read_back_is_stable():
+    """写出去再读回来，变换不该漂移（避免精度不足导致反复重标定）。"""
+    import yaml
+
+    rng = np.random.default_rng(21)
+    R = _rot("x", 37.0) @ _rot("z", -14.0)
+    t = rng.uniform(-0.2, 0.2, (3, 1))
+    txt = format_transform_yaml(R, t, "roundtrip")
+    T = np.array(yaml.safe_load(txt)["T_4x4"], float)
+    assert np.allclose(T[:3, :3], R, atol=1e-7)
+    assert np.allclose(T[:3, 3], t.reshape(3), atol=1e-7)
+
+
+# --------------------------------------------------------------------------
+# 位姿占位检测（决定哪些点参与标定）
+# --------------------------------------------------------------------------
+
+
+def test_is_pose_filled_rejects_placeholder_zeros():
+    assert not is_pose_filled([0.0] * 7)
+
+
+def test_is_pose_filled_rejects_nan():
+    assert not is_pose_filled([float("nan")] * 7)
+
+
+def test_is_pose_filled_rejects_wrong_length():
+    assert not is_pose_filled([0.1, 0.2, 0.3])
+    assert not is_pose_filled([0.1] * 8)
+
+
+def test_is_pose_filled_accepts_real_pose():
+    assert is_pose_filled([-1.107, -0.744, 0.982, 1.856, -0.735, 0.947, -0.118])