93 lines
4.7 KiB
Python
93 lines
4.7 KiB
Python
"""手眼标定·联合求解:从触碰样本同时解 指尖偏移 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()
|