140 lines
4.5 KiB
Python
140 lines
4.5 KiB
Python
# coding=utf-8
|
||
|
||
"""
|
||
眼在手上：使用采集的图片信息和机械臂位姿信息计算
|
||
相机坐标系相对于机械臂末端坐标系的旋转矩阵和平移向量
|
||
"""
|
||
|
||
import os
|
||
import logging
|
||
import yaml
|
||
import cv2
|
||
import numpy as np
|
||
from scipy.spatial.transform import Rotation as R
|
||
|
||
np.set_printoptions(precision=8, suppress=True)
|
||
|
||
|
||
current_path = os.path.dirname(os.path.abspath(__file__))
|
||
images_path = os.path.join(current_path, "data/20250804-1331")
|
||
|
||
# 加载标定板参数
|
||
with open("./config/config.yaml", 'r', encoding='utf-8') as file:
|
||
data = yaml.safe_load(file)
|
||
|
||
XX = data.get("checkerboard_args").get("XX")
|
||
YY = data.get("checkerboard_args").get("YY")
|
||
L = data.get("checkerboard_args").get("L")
|
||
|
||
def gen_T_txt():
|
||
joint_record_file = os.path.join(images_path, "joints_record.txt")
|
||
with open(joint_record_file, "r") as f:
|
||
lines = f.readlines()
|
||
for line in lines:
|
||
temp = line.replace("\n", "").split(" ")
|
||
index = temp[0]
|
||
joints = temp[2:]
|
||
cmd = " ".join(["fk_example", "right"] + joints) + f" -o ./temp/T_ee_{index}.txt"
|
||
print(cmd)
|
||
os.system(cmd)
|
||
print("gen_T_txt finished")
|
||
|
||
def load_robot_poses_from_fk(folder):
|
||
"""
|
||
从fk_example输出的多个T_ee_{index}.txt文件构建RobotToolPose矩阵
|
||
输出: 3x(4*N) numpy矩阵
|
||
"""
|
||
files = [f for f in os.listdir(folder) if f.startswith("T_ee_") and f.endswith(".txt")]
|
||
if not files:
|
||
raise RuntimeError("未找到任何 T_ee_xxx.txt 文件")
|
||
|
||
# 根据index排序
|
||
files.sort(key=lambda x: int(x.split('_')[-1].split('.')[0]))
|
||
|
||
poses = []
|
||
for fname in files:
|
||
path = os.path.join(folder, fname)
|
||
data = np.loadtxt(path, delimiter=',')
|
||
if data.shape != (12,):
|
||
raise RuntimeError(f"{fname} 格式错误，期望12个值(r11,r12,r13,tx,...,tz)")
|
||
|
||
# 3x4矩阵
|
||
mat = data.reshape(3, 4)
|
||
poses.append(mat)
|
||
|
||
# 拼接成 3x(4*N)
|
||
robot_pose_matrix = np.hstack(poses)
|
||
out_path = os.path.join(folder, "../RobotToolPose.csv")
|
||
np.savetxt(out_path, robot_pose_matrix, delimiter=',', fmt='%.8f')
|
||
print(f"RobotToolPose.csv 已生成: {out_path}")
|
||
return out_path
|
||
|
||
|
||
def func():
|
||
path = os.path.dirname(__file__)
|
||
# 设置寻找亚像素角点的参数
|
||
criteria = (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001)
|
||
|
||
# 获取标定板角点的位置
|
||
objp = np.zeros((XX * YY, 3), np.float32)
|
||
objp[:, :2] = np.mgrid[0:XX, 0:YY].T.reshape(-1, 2)
|
||
objp = L * objp
|
||
|
||
obj_points = [] # 存储3D点
|
||
img_points = [] # 存储2D点
|
||
|
||
images_num = [f for f in os.listdir(images_path) if f.endswith('.png')]
|
||
|
||
for i in range(1, len(images_num) + 1):
|
||
image_file = os.path.join(images_path, f"img_{i}.png")
|
||
if os.path.exists(image_file):
|
||
print(f'读 {image_file}')
|
||
img = cv2.imread(image_file)
|
||
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
|
||
|
||
size = gray.shape[::-1]
|
||
ret, corners = cv2.findChessboardCorners(gray, (XX, YY), None)
|
||
|
||
if ret:
|
||
cv2.drawChessboardCorners(img, (XX, YY), corners, ret)
|
||
|
||
# 显示图像窗口
|
||
save_path = os.path.join(current_path, "temp", f"res_{i}.jpg")
|
||
cv2.imwrite(save_path, img)
|
||
|
||
obj_points.append(objp)
|
||
corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria)
|
||
img_points.append(corners2 if corners2 is not None else corners)
|
||
|
||
N = len(img_points)
|
||
|
||
# 标定得到图案在相机坐标系下的位姿
|
||
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, size, None, None)
|
||
|
||
print("-----------------------------------------------------")
|
||
|
||
# === 新增：读取fk_example结果构建RobotToolPose ===
|
||
csv_file = load_robot_poses_from_fk(os.path.join(path, "temp"))
|
||
tool_pose = np.loadtxt(csv_file, delimiter=',')
|
||
|
||
R_tool = []
|
||
t_tool = []
|
||
for i in range(int(N)):
|
||
R_tool.append(tool_pose[0:3, 4*i:4*i+3])
|
||
t_tool.append(tool_pose[0:3, 4*i+3])
|
||
|
||
R_, t_ = cv2.calibrateHandEye(R_tool, t_tool, rvecs, tvecs, cv2.CALIB_HAND_EYE_TSAI)
|
||
|
||
return R_, t_
|
||
|
||
|
||
if __name__ == '__main__':
|
||
gen_T_txt()
|
||
rotation_matrix, translation_vector = func()
|
||
|
||
rotation = R.from_matrix(rotation_matrix)
|
||
quaternion = rotation.as_quat()
|
||
|
||
print(f"旋转矩阵是:\n {rotation_matrix}")
|
||
print(f"平移向量是:\n {translation_vector}")
|
||
print(f"四元数是：\n {quaternion}") |