# coding=utf-8 """ 眼在手上 用采集到的图片信息和机械臂位姿信息计算 相机坐标系相对于机械臂末端坐标系的 旋转矩阵和平移向量 A2^{-1}*A1*X=X*B2*B1^{−1} """ import os import logging import yaml import cv2 import numpy as np from scipy.spatial.transform import Rotation as R from libs.auxiliary import find_latest_data_folder from libs.log_setting import CommonLog from save_poses import poses_main np.set_printoptions(precision=8,suppress=True) logger_ = logging.getLogger(__name__) logger_ = CommonLog(logger_) current_path = os.path.join(os.path.dirname(os.path.abspath(__file__)),"eye_hand_data") images_path = os.path.join("eye_hand_data",find_latest_data_folder(current_path)) file_path = os.path.join(images_path,"RobotToolPose.csv") #采集标定板图片时对应的机械臂末端的齐次变换矩阵 从 第一行到最后一行 需要和采集的标定板的图片顺序进行对应 with open("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") #标定板一格的长度 单位为米 import numpy as np def eulerZYXToRotationMatrix(rx, ry, rz): # 绕 x 轴的旋转矩阵 R_x = np.array([[1, 0, 0], [0, np.cos(rx), -np.sin(rx)], [0, np.sin(rx), np.cos(rx)]]) # 绕 y 轴的旋转矩阵 R_y = np.array([[np.cos(ry), 0, np.sin(ry)], [0, 1, 0], [-np.sin(ry), 0, np.cos(ry)]]) # 绕 z 轴的旋转矩阵 R_z = np.array([[np.cos(rz), -np.sin(rz), 0], [np.sin(rz), np.cos(rz), 0], [0, 0, 1]]) # 使用 @ 运算符进行矩阵乘法：R = R_x @ R_y @ R_z return R_x @ R_y @ R_z def eulerXYZToRotationMatrix(rx, ry, rz): # 绕 x 轴的旋转矩阵 R_x = np.array([[1, 0, 0], [0, np.cos(rx), -np.sin(rx)], [0, np.sin(rx), np.cos(rx)]]) # 绕 y 轴的旋转矩阵 R_y = np.array([[np.cos(ry), 0, np.sin(ry)], [0, 1, 0], [-np.sin(ry), 0, np.cos(ry)]]) # 绕 z 轴的旋转矩阵 R_z = np.array([[np.cos(rz), -np.sin(rz), 0], [np.sin(rz), np.cos(rz), 0], [0, 0, 1]]) # 使用 @ 运算符进行矩阵乘法：R = R_x @ R_y @ R_z return R_z @ R_y @ R_x def rotationMatrixToEulerZYX(R): # 提取 ry（绕 Y 轴的旋转角度） ry = np.arcsin(R[0, 2]) # R(0, 2) = sin(ry) cy = np.cos(ry) if np.abs(cy) > 1e-6: # 正常情况 rx = np.arctan2(-R[1, 2], R[2, 2]) rz = np.arctan2(-R[0, 1], R[0, 0]) else: # 万向节锁：cy ≈ 0 rx = 0 # 任意选择 if ry > 0: rz = np.arctan2(R[1, 0], R[1, 1]) else: rz = np.arctan2(-R[1, 0], R[1, 1]) return np.array([rx, ry, rz]) def rotationMatrixToEulerXYZ(R): # 提取 ry（绕 Y 轴的旋转角度） ry = np.arcsin(-R[2, 0]) # R(2, 0) = -sin(ry) cy = np.cos(ry) if np.abs(cy) > 1e-6: # 正常情况 rx = np.arctan2(R[2, 1], R[2, 2]) rz = np.arctan2(R[1, 0], R[0, 0]) else: # 万向节锁：cy ≈ 0 rx = 0 # 任意选择 if ry > 0: rz = np.arctan2(-R[1, 2], R[1, 1]) else: rz = np.arctan2(R[1, 2], R[1, 1]) return np.array([rx, ry, rz]) def func(): # 设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001 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) # 将世界坐标系建在标定板上，所有点的Z坐标全部为0，所以只需要赋值x和y objp = L*objp obj_points = [] # 存储3D点 img_points = [] # 存储2D点 images_num = [f for f in os.listdir(images_path) if f.endswith('.jpg')] for i in range(1, len(images_num) + 1): #标定好的图片在images_path路径下，从0.jpg到x.jpg image_file = os.path.join(images_path,f"{i}.jpg") if os.path.exists(image_file): logger_.info(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: obj_points.append(objp) corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria) # 在原角点的基础上寻找亚像素角点 if [corners2]: img_points.append(corners2) else: img_points.append(corners) # 绘制角点并保存图片 cv2.drawChessboardCorners(img, (XX, YY), corners2 if corners2 is not None else corners, ret) corner_folder = os.path.join(images_path, "corner") os.makedirs(corner_folder, exist_ok=True) # 如果不存在就创建 save_path = os.path.join(corner_folder, f"corner_{i}.jpg") cv2.imwrite(save_path, img) logger_.info(f"保存带角点的图片到: {save_path}") N = len(img_points) # 标定,得到图案在相机坐标系下的位姿 ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, size, None, None) # logger_.info(f"内参矩阵:\n:{mtx}" ) # 内参数矩阵 # logger_.info(f"畸变系数:\n:{dist}") # 畸变系数 distortion cofficients = (k_1,k_2,p_1,p_2,k_3) print("-----------------------------------------------------") tool_pose = np.loadtxt(file_path,delimiter=',') R_tool = [] t_tool = [] N = tool_pose.shape[0] // 4 # 矩阵个数，每个矩阵占4行 for i in range(N): mat = tool_pose[4*i:4*i+4, :] # 取第i个4x4矩阵 # # 提取旋转矩阵 # R_zyx = mat[0:3, 0:3] # # # 将 ZYX 顺序的旋转矩阵转换为欧拉角（ZYX 顺序） # euler_zyx = rotationMatrixToEulerZYX(R_zyx) # # # 将欧拉角转换为 XYZ 顺序的旋转矩阵 # R_xyz = eulerXYZToRotationMatrix(euler_zyx[0], euler_zyx[1], euler_zyx[2]) # # # 将转换后的旋转矩阵添加到 R_tool # R_tool.append(R_xyz) # # # # 将 ZYX 顺序的旋转矩阵转换为欧拉角（返回值是 rx, ry, rz） # # euler_zyx = R.from_matrix(R_zyx).as_euler('zyx', degrees=False) # # # # # 将欧拉角重新转换为 XYZ 顺序的旋转矩阵 # # R_xyz = R.from_euler('xyz', euler_zyx, degrees=False).as_matrix() # # # # # 将转换后的旋转矩阵添加到 R_tool # # R_tool.append(R_xyz) R_tool.append(mat[0:3, 0:3]) # 提取旋转矩阵 t_tool.append(mat[0:3, 3]) # 提取平移向量 R1, t = cv2.calibrateHandEye(R_tool, t_tool, rvecs, tvecs, cv2.CALIB_HAND_EYE_TSAI) return R1,t if __name__ == '__main__': # 旋转矩阵 rotation_matrix, translation_vector = func() # 将旋转矩阵转换为四元数 rotation = R.from_matrix(rotation_matrix) quaternion = rotation.as_quat() x, y, z = translation_vector.flatten() logger_.info(f"旋转矩阵是:\n { rotation_matrix}") logger_.info(f"平移向量是:\n { translation_vector}") logger_.info(f"四元数是：\n { quaternion}")