132 lines
7.1 KiB
Python
132 lines
7.1 KiB
Python
"""手眼标定(方法 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 storage import save_json
|
||
from pathlib import Path
|
||
import numpy as np, cv2
|
||
|
||
ROOT = Path(__file__).resolve().parents[1]
|
||
|
||
|
||
def draw_alignment(img, cp, cols, rows, arm):
|
||
result = img.copy()
|
||
for k in range(cols * rows):
|
||
if k % cols == cols - 1 or k // cols == rows - 1:
|
||
continue
|
||
# Left arm uses the other diagonal of the same cell. The physical
|
||
# target remains its centre, independent of the alignment direction.
|
||
pa, pb = ((cp[k], cp[k + cols + 1]) if arm == 'left'
|
||
else (cp[k + 1], cp[k + cols]))
|
||
mid = (cp[k + 1] + cp[k + cols]) / 2
|
||
cv2.line(result, tuple(pa.astype(int)), tuple(pb.astype(int)),
|
||
(0, 200, 255), 2, cv2.LINE_AA)
|
||
cv2.circle(result, tuple(mid.astype(int)), 3, (0, 255, 0), -1)
|
||
cv2.putText(result, str(k), (int(mid[0]) - 6, int(mid[1]) + 5),
|
||
cv2.FONT_HERSHEY_SIMPLEX, .5, (255, 0, 0), 2, cv2.LINE_AA)
|
||
cv2.putText(result, f'{arm.upper()} ARM', (15, 30),
|
||
cv2.FONT_HERSHEY_SIMPLEX, .8, (0, 200, 255), 2, cv2.LINE_AA)
|
||
return result
|
||
|
||
|
||
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 个数逗号分隔(行主序)")
|
||
ap.add_argument("--arm", choices=["left", "right"], default="left")
|
||
ap.add_argument("--alignment-only", action="store_true", help="仅重绘对准线,不修改标定数据")
|
||
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
|
||
cp = corners.reshape(-1, 2)
|
||
for side in ('left', 'right'):
|
||
guide = draw_alignment(img, cp, c, r, side)
|
||
cv2.imwrite(str(cap / f'align_lines_{side}.png'), guide)
|
||
if side == a.arm:
|
||
cv2.imwrite(str(cap / 'align_lines.png'), guide)
|
||
print(f"{a.arm}臂对准线 -> {cap}/align_lines_{a.arm}.png")
|
||
if a.alignment_only:
|
||
return
|
||
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]
|
||
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(cap / "corners_labeled.png"), lab)
|
||
vis = img.copy(); cv2.drawChessboardCorners(vis, grid, corners, True)
|
||
cv2.imwrite(str(cap / "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"叠加图 -> {cap}/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),供触碰求解用
|
||
save_json(cap / "camera.json", meta)
|
||
print(f"角点编号叠加图 -> {cap}/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)"
|
||
save_json(cap / "camera.json", meta)
|
||
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()
|