12 KiB
URDF 标定:当前采集实现与失败原因
日期:2026-09-28。描述对象是 src/linkerhand_calibration 的当前实现。
当前正式门限:重投影 1.5 px,区分容差 0.03 px,Holm 校正后的比较概率 ≤ 0.01,转轴不确定度约 3°,转心 3 mm。两个候选的像素误差差不多时,当前实现不选取误差略小的那个。原始 URDF 不被覆盖,修正结果另存。
1. 当前采集顺序,以及停在哪一步
终端看起来像在采角度。每个关节按这个顺序执行:
- 准备运动:只转这一个关节,相机连续拍贴纸。
- 零位几何:用这段图像解出唯一的转轴模型,然后冻结零位。
- 扫行程和稳态点:在声明的指令上记下「指令 ↔ 观测角」。
- 多轮训练和验证。
- 全部通过才拟合,写出 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 |
终端上的中文原因 |