orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
新建 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 的正交阵（排除镜像解）
- 退化输入（全同位姿）不得悄悄给出"看起来能用"的结果
- 稳定性判据：窗口未满不判稳、丢帧后重新计数、抖动超阈不判稳
- 写盘再读回不漂移
This commit is contained in:
arm_dev
2026-09-23 11:46:03 +08:00
parent 52c108de07
commit a3a589b9e3
18 changed files with 735 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/arm_application/arm_application/handeye_calibrate.py
+88 -4
View File
@@ -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
self.declare_parameter('board_rows', 6)
self.declare_parameter('square_size_m', 0.025)
# 默认值必须与 site.yaml 的 handeye.board_* 及结果文件头里的实际板子一致。
# 真实使用的板：内角点 8x11，方格 15mm（见 handeye/results/*.yaml 的头部注释）。
# 这里原先写的是 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
+3 -1
View File
@@ -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_calibrate.yaml
-80
View File
@@ -1,80 +0,0 @@
# Hand-eye auto calibration config.
#
# Workspace: ~/ros2
#
# Teach poses, then solve (prefer the wrapper):
# bash ~/ros2/scripts/run_handeye.sh teach
# bash ~/ros2/scripts/run_handeye.sh merge
# bash ~/ros2/scripts/run_handeye.sh calibrate
handeye_calibrate:
ros__parameters:
mode: "eye_in_hand"
joint_prefix: "Rizon4-063989_"
joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
velocity_scale: 0.10
action_name: "follow_joint_trajectory"
move_time_sec: 4.0
start_index: 0
min_poses: 5
# Camera on flange (eye-in-hand): D405
camera_source: "d405" # d405 | d435 | manual
camera_serial: ""
camera_namespace: ""
realsense_width: 1280
realsense_height: 720
image_topic: ""
camera_info_topic: ""
camera_fx: 0.0
camera_fy: 0.0
camera_cx: 0.0
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)
board_cols: 11
board_rows: 16
square_size_m: 0.011
settle_sec: 0.4
stable_window: 8
stable_pixel_std: 0.45
stable_timeout_sec: 25.0
output_dir: "/tmp/handeye_calib"
result_file: ""
cleanup_images: true
num_poses: 13
# Taught 2026-08-28 via handeye_teach (backend:=rdk)
waypoint_positions: [
# pose 0
-1.107107, -0.744489, 0.982447, 1.856279, -0.735517, 0.946910, -0.118163,
# pose 1
-0.982764, -1.163516, 1.128527, 1.406787, -0.502416, 0.571857, 0.224024,
# pose 2
-1.012149, -1.180698, 1.184095, 1.390763, -0.559587, 0.407246, 0.249883,
# pose 3
-0.984199, -1.175286, 1.175012, 1.397197, -0.796608, 0.459682, 0.247304,
# pose 4
-0.982355, -1.196864, 1.076502, 1.590187, -1.106339, 0.636800, 0.481410,
# pose 5
-1.121428, -1.173892, 1.152070, 1.762215, -1.147604, 0.641314, 0.434178,
# pose 6
-1.121716, -1.174258, 1.203286, 1.643879, -1.200144, 0.459669, 0.315685,
# pose 7
-1.100360, -1.337467, 1.235742, 1.377341, -1.211692, 0.569701, 0.220985,
# pose 8
-1.160070, -1.473164, 1.364572, 1.369112, -0.784077, 0.489467, 0.378945,
# pose 9
-0.575560, -1.470411, 1.348577, 0.982228, -1.163608, -0.208091, 0.410659,
# pose 10
-0.716065, -1.411332, 1.272539, 1.753754, -1.856130, -0.098214, 0.341287,
# pose 11
-0.870910, -1.412227, 1.243149, 1.900543, -1.886769, 0.153908, -0.399394,
# pose 12
-0.656925, -1.412314, 1.216671, 1.820324, -1.850224, -0.103640, -0.399964,
]
src/arm_application/config/handeye_poses.yaml
-38
View File
@@ -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_poses_89.yaml
-38
View File
@@ -1,38 +0,0 @@
# Hand-eye taught joint poses (7 joints × N). Shared by calibrate + waypoints.
#
# 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_poses_91.yaml
-36
View File
@@ -1,36 +0,0 @@
# Right arm 91 (Rizon4-063991) taught joints for hand-eye.
# Fill with:
# bash ~/ros2/scripts/run_handeye.sh --arm 91 teach
# bash ~/ros2/scripts/run_handeye.sh --arm 91 merge
#
# Do not reuse 89's waypoint_positions.
/**:
ros__parameters:
num_poses: 12
waypoint_positions: [
# pose 0
0.387144, -0.514770, -0.072695, 1.399918, 0.326501, 0.308501, -0.298212,
# pose 1
0.459205, -0.747691, 0.042380, 0.908430, 0.436933, -0.099413, -0.398336,
# pose 2
0.113162, -0.929267, -0.132969, 0.776759, 0.579499, -0.081271, -0.588715,
# pose 3
0.823056, -1.181803, -0.157161, 0.344860, 0.073450, -0.436449, -0.333484,
# pose 4
0.561660, 0.126479, -0.217063, 2.400061, 1.116363, 1.205544, -0.955972,
# pose 5
0.749011, 0.058548, -0.125615, 1.951037, 0.399065, 0.388437, -1.057397,
# pose 6
0.707611, -0.557010, -0.302672, 1.263980, 0.333093, 0.208229, -0.938372,
# pose 7
0.197846, -0.400530, 0.263064, 1.879643, 0.062586, 1.133715, -0.791285,
# pose 8
0.804027, -0.518162, 0.353947, 1.985246, -0.941195, 0.481238, -1.551113,
# pose 9
0.296161, -0.030159, 0.317117, 2.335652, 1.072158, 1.524893, -0.591372,
# pose 10
0.346492, -0.906418, -0.118014, 0.810448, 0.420788, -0.122104, -0.197859,
# pose 11
1.031802, -0.422436, -0.640939, 1.656629, 0.375163, 0.470365, -0.328041,
]
src/arm_application/config/handeye_result.yaml
-14
View File
@@ -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/config/handeye_result_89.yaml
-14
View File
@@ -1,14 +0,0 @@
# Left arm 89 (Rizon4-063989) eye-in-hand. Do not use for 91.
# 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/config/handeye_result_91.yaml
-13
View File
@@ -1,13 +0,0 @@
# Hand-eye result
# mode: eye_in_hand
# camera_source: d405
# views: 11
# utc: 2026-09-15T08:44:59.380472+00:00
# board: 8x11 square_m=0.015
# T_cam_to_gripper (eye_in_hand)
translation_m: [-0.06856274, -0.00768546, 0.13773073]
T_4x4:
- [0.00611182, -0.87098536, 0.49127096, -0.06856274]
- [0.99997742, 0.00395004, -0.00543744, -0.00768546]
- [0.00279539, 0.49129310, 0.87098983, 0.13773073]
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
src/arm_application/config/handeye_waypoints.yaml
-25
View File
@@ -1,25 +0,0 @@
# Hand-eye interactive waypoint tour (preview + Free Drive confirm).
# Poses: sibling handeye_poses.yaml
#
# Run:
# bash ~/ros2/scripts/run_handeye.sh waypoints
handeye_waypoints:
ros__parameters:
joint_prefix: "Rizon4-063989_"
joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
velocity_scale: 0.05
action_name: "follow_joint_trajectory"
dwell_sec: 2.5
capture_mode: "preview" # auto | keypress | preview
start_index: 0
log_csv: ""
capture_topic: "handeye/capture"
camera_source: "d405"
camera_namespace: ""
image_topic: ""
preview_window: true
append_poses_file: "/tmp/handeye_calib/taught_poses.txt"
waypoint_times: [4.0, 4.0, 4.0, 4.0, 4.0, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.0, 4.0]
src/arm_application/launch/visual_servo.launch.py
+2 -2
View File
@@ -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
+383
View File
@@ -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])