Files
----1.0/URDF_IMAGE_SOLVE_HANDOFF.md
T

12 KiB
Raw Blame History

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 终端上的中文原因