Files

93 lines
4.7 KiB
Python
Raw Permalink Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
"""手眼标定·联合求解:从触碰样本同时解 指尖偏移 d 和 标定板位姿 T_base_board,再算 T_base_cam 写回 camera.json。
模型:食指尖碰第 k 号角点时,指尖基座坐标 = 该角点基座坐标:
t_i + R_i·d = R_bb·b_i + t_bb (R_i,t_i = arm_state.pose;b_i = 角点板系坐标 objp[k])
未知:R_bb(3) t_bb(3) d(3) = 9。≥4 个不同角点、手腕朝向有变化即可解(朝向不变则 d 不可观)。
然后 T_base_cam = T_base_board · inv(T_cam_board),写进 camera.json 外参。
用法:.venv/bin/python scripts/solve_base_board.py --capture data/real_calib
"""
import argparse, json
from storage import save_json
from arm_contract import check_sample_arm
from pathlib import Path
import numpy as np
from scipy.optimize import least_squares
from scipy.spatial.transform import Rotation as Rot
ROOT = Path(__file__).resolve().parents[1]
def residual(x, Rs, ts, bs):
R_bb = Rot.from_rotvec(x[0:3]).as_matrix(); t_bb = x[3:6]; d = x[6:9]
return np.concatenate([ti + Ri @ d - (R_bb @ bi + t_bb) for Ri, ti, bi in zip(Rs, ts, bs)])
def main():
ap = argparse.ArgumentParser()
ap.add_argument("--capture", default="data/real_calib")
ap.add_argument("--arm", choices=["left", "right"], default="left")
a = ap.parse_args()
cap = ROOT / a.capture
cam = json.loads((cap / "camera.json").read_text())
assert "board_objp" in cam and "T_cam_board" in cam, "camera.json 缺 board_objp/T_cam_board,先跑 handeye_solve.py"
objp = np.array(cam["board_objp"], float); T_cb = np.array(cam["T_cam_board"], float)
C, R = cam["board_grid"] # 列, 行(8, 5)
data = json.loads((cap / "touch_samples.json").read_text())
check_sample_arm(data, a.arm)
samples = data["samples"]
ncor = len({s["corner"] for s in samples})
assert ncor >= 4, f"只有 {ncor} 个不同角点,至少要 4 个"
def target(k):
"""锚点 k 的目标板系点 = 其右下方格中心 = (角点 k+1 + 角点 k+C)/2(法兰侧边对准 k+1~k+C 对角线中点)。"""
assert k % C < C - 1 and k // C < R - 1, f"角点 {k} 在最右列/最底行、没有右下方格,不能作锚点(用 0~6/8~14/16~22/24~30)"
return (objp[k + 1] + objp[k + C]) / 2
Rs = [Rot.from_quat(s["quat_xyzw"]).as_matrix() for s in samples]
ts = [np.array(s["pos"], float) for s in samples]
bs = [target(s["corner"]) for s in samples]
# 朝向变化幅度(d 可观性):样本旋转两两最大夹角
spread = 0.0
for i in range(len(Rs)):
for j in range(i + 1, len(Rs)):
ang = np.rad2deg(np.linalg.norm(Rot.from_matrix(Rs[i].T @ Rs[j]).as_rotvec()))
spread = max(spread, ang)
print(f"样本 {len(samples)} 条 / {ncor} 个角点;手腕朝向最大变化 {spread:.0f}°" +
("" if spread > 15 else " ⚠ 朝向变化过小,d 可能解不准——多换几个朝向再碰"))
# 多 yaw 初值防局部最优
best = None
for yaw in np.deg2rad([0, 90, 180, 270]):
R0 = Rot.from_euler("z", yaw)
t0 = np.mean([ti - R0.as_matrix() @ bi for ti, bi in zip(ts, bs)], axis=0)
x0 = np.concatenate([R0.as_rotvec(), t0, np.zeros(3)])
sol = least_squares(residual, x0, args=(Rs, ts, bs), method="lm")
if best is None or sol.cost < best.cost:
best = sol
x = best.x
R_bb = Rot.from_rotvec(x[0:3]).as_matrix(); t_bb = x[3:6]; d = x[6:9]
res = residual(x, Rs, ts, bs).reshape(-1, 3)
err = np.linalg.norm(res, axis=1) * 1000
print(f"拟合残差(每次触碰): 均值 {err.mean():.2f}mm 最大 {err.max():.2f}mm (<3mm 好)")
print(f"解出的食指尖偏移 d(Arm_Tip 系, mm) = {(d*1000).round(1).tolist()}")
T_bb = np.eye(4); T_bb[:3, :3] = R_bb; T_bb[:3, 3] = t_bb
T_base_cam = T_bb @ np.linalg.inv(T_cb)
cam["T_world_cam_opencv"] = T_base_cam.tolist(); cam["cam_pos_world"] = T_base_cam[:3, 3].tolist()
cam["T_base_cam_opencv"] = T_base_cam.tolist()
cam["calibration_arm"] = a.arm
cam["calibration_pose_topic"] = f"/{a.arm}/arm_state"
cam["coordinate_frame"] = "base_link"
cam["T_base_board"] = T_bb.tolist(); cam["fingertip_offset_m"] = d.tolist()
cam["extrinsic_status"] = f"calibrated(touch joint-solve, {ncor}pts, res {err.mean():.1f}mm)"
save_json(cap / "camera.json", cam)
print(f"\nT_base_cam(相机在基座系, m):\n{np.array2string(T_base_cam, precision=4, suppress_small=True)}")
print(f"相机在基座系位置: {T_base_cam[:3,3].round(3).tolist()} → 已写入 {cap/'camera.json'} 外参(T_world_cam_opencv)")
print(f"下一步:bash run.sh validate --arm {a.arm},用同一固定参考点做独立验证(只读,不自动运动)。")
if __name__ == "__main__":
main()