Files

110 lines
6.4 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.
"""手眼标定(方法 A:板平放桌上)求解器。
① 检测棋盘格 → solvePnP → T_cam_board(板在相机系位姿,OpenCV 约定)。
② 若给 --base-board(T_base_board 4x4,板在机器人基座系位姿):
T_base_cam = T_base_board @ inv(T_cam_board) → 写进 <capture>/camera.json 的外参,供 perceive_object 用。
棋盘格朝向消歧:板平放桌上、法线朝相机——强制 T_cam_board 的 z 轴(板法线)指向相机(cam→board 连线反向)。
用法:.venv/bin/python scripts/handeye_solve.py --capture data/real_calib --square 0.027 [--cols 9 --rows 6] \
[--base-board "r00,r01,...,1"] # 16 个数行主序
"""
import argparse, json
from pathlib import Path
import numpy as np, cv2
ROOT = Path(__file__).resolve().parents[1]
def load_K(cap):
fx, fy, cx, cy = map(float, (cap / "intrinsics.txt").read_text().split())
return np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1.0]])
def _pnp_err(objp, cor, K):
ok, rv, tv = cv2.solvePnP(objp, cor, K, None, flags=cv2.SOLVEPNP_ITERATIVE)
pr, _ = cv2.projectPoints(objp, rv, tv, K, None)
e = float(np.linalg.norm(pr.reshape(-1, 2) - cor.reshape(-1, 2), axis=1).mean())
R, _ = cv2.Rodrigues(rv); T = np.eye(4); T[:3, :3] = R; T[:3, 3] = tv.ravel()
return e, T
def detect_grid(gray, K, square, cols, rows):
"""扫描候选网格,对每个跑 solvePnP,取重投影误差最小者(自动拒绝误检/转置)。"""
flags = cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
cand = [(cols, rows)] if (cols and rows) else [(c, r) for c in range(4, 14) for r in range(3, 12) if c > r]
best = None
for (c, r) in cand:
ok, cor = cv2.findChessboardCorners(gray, (c, r), flags=flags)
if not ok:
continue
cor = cv2.cornerSubPix(gray, cor, (11, 11), (-1, -1),
(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-3))
objp = np.zeros((c * r, 3), np.float32); objp[:, :2] = np.mgrid[0:c, 0:r].T.reshape(-1, 2) * square
e, T = _pnp_err(objp, cor, K)
if best is None or e < best[0]:
best = (e, (c, r), cor, T)
return best # (err, grid, corners, T_cam_board) 或 None
def main():
ap = argparse.ArgumentParser()
ap.add_argument("--capture", required=True); ap.add_argument("--square", type=float, required=True, help="方格边长(m)")
ap.add_argument("--cols", type=int, default=None); ap.add_argument("--rows", type=int, default=None)
ap.add_argument("--base-board", default=None, help="T_base_board 4x4,16 个数逗号分隔(行主序)")
a = ap.parse_args()
cap = Path(a.capture).expanduser().resolve()
K = load_K(cap)
img = cv2.imread(str(cap / "rgb.png")); gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
best = detect_grid(gray, K, a.square, a.cols, a.rows)
if best is None:
raise SystemExit("未检测到完整棋盘格。确认整块板在画面内、够清晰;或用 --cols/--rows 指定内角点网格。")
err, grid, corners, T_cam_board = best
c, r = grid
if err > 2.0:
print(f"⚠ 最佳网格 {c}x{r} 重投影误差仍达 {err:.1f}px,可能板太斜/模糊/被遮,结果不可信——建议重摆再存帧")
# 每个角点的板系坐标(与 solvePnP 的 objp 同序):objp[k] ↔ corners[k]
objp = np.zeros((c * r, 3), np.float32); objp[:, :2] = np.mgrid[0:c, 0:r].T.reshape(-1, 2) * a.square
# 编号叠加图:触碰时按编号 k 碰角点,record_touch 用 k 取 objp[k]
cp = corners.reshape(-1, 2)
lab = img.copy()
for k, px in enumerate(cp):
cv2.circle(lab, (int(px[0]), int(px[1])), 3, (0, 255, 0), -1)
cv2.putText(lab, str(k), (int(px[0]) + 4, int(px[1]) - 4), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 0, 255), 1, cv2.LINE_AA)
cv2.imwrite(str(ROOT / "out/corners_labeled.png"), lab)
# 对准线图:锚点 k 的右下方格对角线(k+1 - k+8),法兰侧边对齐这条线;标注 k 在其中点(=目标方格中心)
alab = img.copy()
for k in range(c * r):
if k % c < c - 1 and k // c < r - 1: # 有右邻 k+1、下邻 k+c
pa = cp[k + 1]; pb = cp[k + c]; mid = (pa + pb) / 2
cv2.line(alab, (int(pa[0]), int(pa[1])), (int(pb[0]), int(pb[1])), (0, 200, 255), 2, cv2.LINE_AA)
cv2.circle(alab, (int(mid[0]), int(mid[1])), 3, (0, 255, 0), -1)
cv2.putText(alab, str(k), (int(mid[0]) - 6, int(mid[1]) + 5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 0, 0), 2, cv2.LINE_AA)
cv2.imwrite(str(ROOT / "out/align_lines.png"), alab)
vis = img.copy(); cv2.drawChessboardCorners(vis, grid, corners, True)
cv2.imwrite(str(ROOT / "out/handeye_detect.png"), vis)
print(f"检测到棋盘格 {c}x{r} 内角点,方格 {a.square*1000:.0f}mm;重投影误差 = {err:.2f} px(<0.5 好)")
print(f"T_cam_board(相机系, m):\n{np.array2string(T_cam_board, precision=4, suppress_small=True)}")
print(f"板中心在相机系: {(T_cam_board[:3,3]).round(3).tolist()} 距离 {np.linalg.norm(T_cam_board[:3,3])*100:.1f}cm")
print(f"叠加图 -> out/handeye_detect.png")
# 持久化 T_cam_board + 板规格,供下一步 solve_base_board 用
meta = json.loads((cap / "camera.json").read_text())
meta["T_cam_board"] = T_cam_board.tolist(); meta["board_grid"] = [c, r]
meta["board_square_m"] = a.square; meta["board_reproj_px"] = round(err, 3)
meta["board_objp"] = objp.tolist() # 角点编号 k → 板系坐标(m),供触碰求解用
(cap / "camera.json").write_text(json.dumps(meta, indent=2))
print(f"角点编号叠加图 -> out/corners_labeled.png(触碰时按编号 0~{c*r-1} 碰角点)")
if a.base_board:
Tbb = np.array([float(x) for x in a.base_board.replace("[", "").replace("]", "").split(",")]).reshape(4, 4)
T_base_cam = Tbb @ np.linalg.inv(T_cam_board)
meta = json.loads((cap / "camera.json").read_text())
meta["T_world_cam_opencv"] = T_base_cam.tolist(); meta["cam_pos_world"] = T_base_cam[:3, 3].tolist()
meta["extrinsic_status"] = "calibrated(handeye method A)"
(cap / "camera.json").write_text(json.dumps(meta, indent=2))
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'} 外参")
if __name__ == "__main__":
main()