186 lines
12 KiB
Markdown
186 lines
12 KiB
Markdown
# URDF 标定:当前采集实现与失败原因
|
||
|
||
日期:2026-09-28。描述对象是 `src/linkerhand_calibration` 的当前实现。
|
||
|
||
当前正式门限:重投影 1.5 px,区分容差 0.03 px,Holm 校正后的比较概率 ≤ 0.01,转轴不确定度约 3°,转心 3 mm。两个候选的像素误差差不多时,当前实现不选取误差略小的那个。原始 URDF 不被覆盖,修正结果另存。
|
||
|
||
---
|
||
|
||
## 1. 当前采集顺序,以及停在哪一步
|
||
|
||
终端看起来像在采角度。每个关节按这个顺序执行:
|
||
|
||
1. 准备运动:只转这一个关节,相机连续拍贴纸。
|
||
2. 零位几何:用这段图像解出唯一的转轴模型,然后冻结零位。
|
||
3. 扫行程和稳态点:在声明的指令上记下「指令 ↔ 观测角」。
|
||
4. 多轮训练和验证。
|
||
5. 全部通过才拟合,写出 JSON,再另存修正 URDF。
|
||
|
||
第 2 步不通过时,这个关节的后续样本不算有效标定数据,也不进入 URDF。手在动、SDK 有反馈,只表示指令回路正常。标定用的角度来自视觉相对姿态,当前实现不把反馈字节当成关节角。
|
||
|
||
暂停发生在 `runtime/coordinator.py` 的 `joint_zero` 阶段、调用 `freeze_zeros` 之前。几何求解有错误就暂停,原因字符串是 `joint_zero_motion_unresolved:` 加上具体几何错误。这一步的计算上限是 120 秒,超时原因是 `joint_zero_motion_unresolved:image_motion_solver_timeout`。
|
||
|
||
|
||
| 终端里看到 | 停在哪一步 | 此时的状态 |
|
||
| ------------------------------------------- | ---------- | ---------------- |
|
||
| 采集并冻结关节零位,或 `OBS-POSE` / `OBS-JOINT-BRANCH` | 第 2 步,零位几何 | 这段运动还没有记成指令–角度样本 |
|
||
| `joint_zero_unavailable` | 第 2 步 | 零位图像不够,或基准对不上 |
|
||
| 正式扫描 / 稳态指令映射 | 第 3 步 | 零位已经冻结 |
|
||
| `sweep_quality_failed` | 第 3 步 | 声明的稳态点没有采够 |
|
||
| 整场通过且 `publication_allowed` | 第 5 步之后 | 此时才写出修正 URDF |
|
||
|
||
|
||
采集停住,主要发生在第 2 步:没有选出唯一可用的图像模型。`sweep_quality_failed` 是第 3 步的另一类失败:零位已经通过,计划中的稳态指令点没有采够。
|
||
|
||
---
|
||
|
||
## 2. 第 2 步当前怎么求解
|
||
|
||
相机只提供贴纸四个角的像素。求解分两小步。
|
||
|
||
### 2.1 单帧:每张贴纸两个朝向
|
||
|
||
`core/geometry/tag_pose/ippe.py` 的 `solve_square_tag_ippe` 使用贴纸边长、相机内参和四个角,调用 OpenCV `SOLVEPNP_IPPE_SQUARE`。
|
||
|
||
一张平面正方形贴纸,单帧通常得到两个朝向。两个朝向投影回画面几乎重合,重投影都可以很小。检测到标签,并不表示朝向已经唯一。
|
||
|
||
角点不合法、内参无效,或解到相机背后时,候选列表为空。`core/geometry/tag_pose/tracking.py` 把这种空结果写成 `pnp_solve_failed`。单帧深度不作为关节角。
|
||
|
||
### 2.2 整段准备视频:拟合一根转轴
|
||
|
||
入口是 `core/geometry/tag_pose/production_image_motion.py` 的 `resolve_image_motion`。铰链拟合在 `core/geometry/tag_pose/motion_image_solver.py` 的 `ImageHingeBundle`。
|
||
|
||
当前模型把这段运动当成一根转轴带着贴纸转:
|
||
|
||
- 转轴 `axis`
|
||
- 转心 `pivot原`
|
||
- 贴纸在骨头上的安装 `mount`
|
||
- 每一帧的转角 `theta`
|
||
|
||
关系是 `R = exp(theta × axis) × R_ref`,平移是 `t = pivot + R × mount`。优化量是模型投影出的四个角与拍到的四个角之差。
|
||
|
||
准备段至少 24 帧。训练图像和留出图像分开,默认各最多 72 帧,训练帧取交错序号。赢家只按训练图像的平方误差选择。留出图像只用来检查这套模型是否唯一;留出误差更小也不会改选另一套。
|
||
|
||
通过之后冻结轴线、参考旋转、转心和安装关系。之后 `image_motion_projection.py` 只对当前帧反解角度,不再重拟合这些几何量。
|
||
|
||
---
|
||
|
||
## 3. 两个朝向如何变成若干套候选
|
||
|
||
一张贴纸的两个朝向是「两套」。若干套候选是每张尚未冻结的贴纸各保留两套朝向,再互相搭配。拟合用的仍是上一节那一个铰链。
|
||
|
||
`core/geometry/tag_pose/motion_evidence.py` 的 `_connect_paths` 按时间把同一张贴纸的两个朝向各自接成一条连续轨迹。两条轨迹几乎是同一个姿态时,合成一条。已经冻结的父贴纸只保留当时选中的那一条。
|
||
|
||
`production_image_motion.py` 对每张贴纸的轨迹编号做笛卡尔积。一个关节至少包含父贴纸和子贴纸。两张都还有两个朝向时是四套:
|
||
|
||
|
||
| 候选 | 父贴纸 | 子贴纸 |
|
||
| --- | ---- | ---- |
|
||
| 1 | 朝向 A | 朝向 A |
|
||
| 2 | 朝向 A | 朝向 B |
|
||
| 3 | 朝向 B | 朝向 A |
|
||
| 4 | 朝向 B | 朝向 B |
|
||
|
||
|
||
父贴纸已经冻结时,只剩子贴纸的两套。三张都未冻结时是 2×2×2。
|
||
|
||
离开第 2 步的条件是:至少一套足够符合这段图像,并且它比其余套明显更好。其余套与它是同一个物理姿态时,当前实现把它们合并并通过。像素上接近、三维上是另一个朝向时,当前实现不通过。
|
||
|
||
---
|
||
|
||
## 4. 失败代号在哪里产生
|
||
|
||
单套候选先在 `production_image_motion.py` 的 `_fit_hypothesis` 里写下自己的原因。全部候选算完后,`select_image_hypotheses` 收成一个总原因。终端上 `joint_zero_motion_unresolved:` 后面是这个总原因。各候选自己的原因留在假设列表里。
|
||
|
||
单套候选按这个顺序被拒绝:
|
||
|
||
|
||
| 条件 | 写在该候选上的原因 |
|
||
| --------------------------- | ----------------------------------- |
|
||
| 优化没收敛,或留出帧没算完 | `image_motion_optimizer_unresolved` |
|
||
| 训练或留出图像里,任一帧角点 RMS > 1.5 px | `image_motion_reprojection_failed` |
|
||
| 转轴不确定度超过约 3°,或转心超过 3 mm | `image_motion_geometry_uncertain` |
|
||
|
||
|
||
带有原因的候选 `training_accepted` 为假,不能成为赢家。
|
||
|
||
汇总成总原因的规则:
|
||
|
||
|
||
| 总原因 | 出现条件 |
|
||
| ------------------------------------------- | ---------------------------------------------------------------------------------- |
|
||
| `image_motion_solver_budget_exhausted` | 没有可接纳的赢家,且有候选用完求解预算(求解状态 0)。操作员代号 `OBS-SOLVER-118`。这表示计算没有完成。 |
|
||
| `image_motion_no_observable_image_model` | 没有任何候选被接纳,或按训练误差选中的那个自己还带失败原因。四套都是 `image_motion_reprojection_failed` 时,终端总原因是这一句。 |
|
||
| `image_motion_families_not_distinguishable` | 已有一套通过 1.5 px 和质量门,留出图像仍不能把它和另一套分开。操作员代号 `OBS-POSE-AMBIGUOUS-117`。 |
|
||
| `image_motion_alternative_not_evaluable` | 另一套没收敛或没有残差。当前实现不把缺失证据当作已经排除。 |
|
||
| `joint_zero_unavailable` | 观测超时,且没有 `branch_initialization.error()`。零位图像或基准本身不够。 |
|
||
|
||
|
||
`image_motion_reprojection_failed` 先写在每一套候选上,再被收成 `image_motion_no_observable_image_model`,通常不单独出现在暂停字符串的最外层。
|
||
|
||
---
|
||
|
||
## 5. 第 2 步的两种失败
|
||
|
||
### 5.1 多套都很像,选不出唯一姿态
|
||
|
||
至少一套把角点贴进 1.5 px,另一套在整段运动上差不多好。当前实现不选取误差略小的那套。
|
||
|
||
总原因:`image_motion_families_not_distinguishable`。
|
||
|
||
比较在 `core/geometry/tag_pose/image_model_selection.py`。留出帧按运动角度分段,比较量是 `另一套的误差² - (选中误差 + 0.03 px)²`。单侧均值检验和符号秩检验都通过后,再对全部比较做 Holm 校正,校正后概率必须 ≤ 0.01。
|
||
|
||
此时标签可以一直被检测到,也在画面里移动。两套不同的三维姿态都能解释同一段像素。相机几乎正对、运动主要在像平面内滑动时,深度差在图像上很小,就会得到这个结果。同一条运动弧增加帧数,比较的仍是同一种投影。
|
||
|
||
### 5.2 每一套都不像,没有可用模型
|
||
|
||
每一套「单轴带着贴纸转」都对不齐这段角点。常见细因是 `image_motion_reprojection_failed`:训练或留出的单帧 RMS 超过 1.5 px。也可能是转轴或转心不确定,或优化没有收敛。全部候选都不能被接纳。
|
||
|
||
总原因:`image_motion_no_observable_image_model`。
|
||
|
||
标签仍然可以检测得到。失败含义是这段图像不能由一个单关节转轴模型解释。出现这个结果时,贴纸可能太侧、角点不稳定、弯曲时还有别的轴在动、贴纸不跟该连杆刚体一起动,或当前视角下的运动不像绕这根轴转。
|
||
|
||
---
|
||
|
||
## 6. 进入铰链拟合之前就被丢掉的单帧
|
||
|
||
这些拒绝发生在 `tracking.py`。被拒绝的帧不进入零位模型:
|
||
|
||
|
||
| 原因 | 含义 |
|
||
| ------------------------------------------- | ------------------------------ |
|
||
| `pnp_solve_failed` | 这一帧没有解出任何三维朝向 |
|
||
| `no_pose_within_reprojection_or_tilt_limit` | 解出了朝向,但重投影超过 1.5 px,或倾角超过约 75° |
|
||
| `pose_jump` | 与已经跟踪的姿态相差过大 |
|
||
|
||
|
||
固定基准锁定之后,当前实现不再重解它的朝向,只把第一次冻结的姿态投影到当前角点上核对。
|
||
|
||
---
|
||
|
||
## 7. 第 3 步的失败
|
||
|
||
`sweep_quality_failed` 在 `coordinator.py` 里、零位冻结之后触发。它检查声明的稳态指令点是否采够,不检查朝向双解。计划中的点数没有走完时停在这里。
|
||
|
||
---
|
||
|
||
## 8. 当前实现所在文件
|
||
|
||
路径相对于 `src/linkerhand_calibration/`。
|
||
|
||
|
||
| 文件 | 当前职责 |
|
||
| -------------------------------------------------------------------------- | -------------------------------------------- |
|
||
| `linkerhand_calibration/core/geometry/tag_pose/ippe.py` | 单帧两个朝向 |
|
||
| `linkerhand_calibration/core/geometry/tag_pose/tracking.py` | 单帧拒绝,含 `pnp_solve_failed` |
|
||
| `linkerhand_calibration/core/geometry/tag_pose/motion_evidence.py` | 把朝向接成连续轨迹 |
|
||
| `linkerhand_calibration/core/geometry/tag_pose/production_image_motion.py` | 候选搭配、单套打原因、汇总总原因 |
|
||
| `linkerhand_calibration/core/geometry/tag_pose/motion_image_solver.py` | 铰链拟合 |
|
||
| `linkerhand_calibration/core/geometry/tag_pose/image_model_selection.py` | 训练选赢家、留出区分 |
|
||
| `linkerhand_calibration/core/geometry/tag_pose/image_motion_projection.py` | 冻结后只反解当前帧角度 |
|
||
| `linkerhand_calibration/core/geometry/tag_pose/parameters.py` | 1.5 px、0.03 px、75° |
|
||
| `linkerhand_calibration/runtime/coordinator.py` | `joint_zero` 暂停,以及之后的 `sweep_quality_failed` |
|
||
| `linkerhand_calibration/runtime/reporting/reasons_zh.py` | 终端上的中文原因 |
|
||
|
||
|