import cv2 import os from datetime import datetime import numpy as np import sys from pathlib import Path import time from scipy.spatial.transform import Rotation as R sys.path.append(str(Path(__file__).resolve().parent.parent)) sys.path.append('/home/jsfb/jsfb_ws/MassageRobot_Dobot/Massage/MassageControl') from hardware.remote_cam import ToolCamera from hardware.dobot_nova5 import dobot_nova5 np.set_printoptions(precision=8, suppress=True) # 设置寻找亚像素角点的参数，采用的停止准则是最大循环次数30和最大误差容限0.001 criteria = (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 0.001) # 标定计算类 # -- 连接AuboC5 # -- 连接 ToolCamera # -- 运动到初始位置 计算下一步运动位姿 # -- 开始手眼标定 # -- 输出结果 class Calibration: def __init__(self, board_size, square_size, file_folder): """ 初始化标定计算类。 board_size: 标定板的格子数，格式为 (width, height) square_size: 格子的大小，单位为米 """ self.board_size = board_size self.square_size = square_size self.file_address = file_folder self.objp = self._create_object_points() self.image_points = [] self.robot_poses = [] self.cam = ToolCamera(host='127.0.0.1') self.cam.start() self.size = None self.intrinsics_mtx = None self.disorder = None time.sleep(2) rgb_image, depth_image, camera_intrinsics = self.cam.get_latest_frame() self.save_directory = '/home/jsfb/jsfb_ws/global_config/captured_images' if rgb_image is None : print(f'===============相机连接失败请检查并重试==============') else: self.size = rgb_image.shape[:2] print(f'===============相机连接成功============== ',self.size) self.intrinsics = camera_intrinsics self.arm = dobot_nova5("192.168.5.1") self.arm.start() self.obj_points = [] print(f'===============机械臂连接成功==============') def _create_object_points(self): """创建标定板的3D点""" objp = np.zeros((self.board_size[0] * self.board_size[1], 3), np.float32) objp[:, :2] = np.mgrid[0:self.board_size[0], 0:self.board_size[1]].T.reshape(-1, 2) * self.square_size return objp def collect_data(self): #移动到初始位置 pose = [-45.0009079,55.5785141,-120.68821716,5.11103201,-90.00195312,-90.00085449] code = self.arm.RunPoint_P_inJoint(pose) if code == -1: print("运动到拍照位置失败") #拍照 self.check_corners() #运动到下一个位置 # 目前只设计了44条轨迹 for i in range (0,18): next_pose = self.get_next_pose_ugly(i) code=self.arm.RunPoint_P_inJoint(next_pose) time.sleep(0.5) if code == -1: print("运动到拍照位置失败") else: self.check_corners() time.sleep(0.5) def check_corners(self): rgb, depth, intrinsics = self.cam.get_latest_frame() # img_path = f'photo{i}' # filename = os.path.join(self.save_directory, f'{img_path}/rgb_image.png') # rgb=cv2.imread(filename) gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2GRAY) ret, corners = cv2.findChessboardCorners(gray, self.board_size, None) if ret: robot_pose = self.get_pose_homomat() self.add_sample(gray,corners,robot_pose) print(f"robot_pose = {robot_pose}") pic_name = self.file_address + "/"+str(len(self.image_points)) + ".jpg" print(f"save path as: {pic_name}") # 绘制检测到的角点 (用于可视化，可选) cv2.drawChessboardCorners(rgb, self.board_size, corners, ret) cv2.imwrite(pic_name, rgb) return True return False def add_sample(self, gray, corners,robot_pose): """ 添加标定样本。 image: 捕获的图像 robot_pose: 机械臂的位姿，格式为 [x, y, z, roll, pitch, yaw] """ # 检测棋盘角点 self.obj_points.append(self.objp) corners2 = cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria) # 在原角点的基础上寻找亚像素角点 if [corners2]: self.image_points.append(corners2) else: self.image_points.append(corners) self.robot_poses.append(robot_pose) def get_pose_homomat(self): arm_position,arm_orientation = self.arm.get_arm_position() rotation_matrix = R.from_quat(arm_orientation).as_matrix() translation_vector = arm_position homo_mat = np.eye(4) # 创建 4x4 单位矩阵 homo_mat[:3, :3] = rotation_matrix homo_mat[:3, 3] = translation_vector return homo_mat def camera_calib(self): # print(f"size is {self.size}") ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(self.obj_points, self.image_points, (self.size[1], self.size[0]), None, None) # print("rvecs,tvecs:",rvecs,tvecs) if ret: print("内参矩阵:\n", mtx) # 内参数矩阵 print("畸变系数:\n", dist) # 畸变系数 distortion cofficients = (k_1,k_2,p_1,p_2,k_3) print("重投影误差:\n",ret) print("++++++++++相机标定完成++++++++++++++") self.intrinsics_mtx = mtx self.disorder = dist else: print("-----------相机标定失败--------------") return rvecs, tvecs def split_robot_pose(self): """ 将包含 4x4 齐次矩阵的 robot_pose 列表拆分成 R_arm 和 t_arm。 Args: robot_pose: 包含 4x4 齐次矩阵的 Python list。 Returns: R_arm: 旋转矩阵的 Python list, 每个元素是 3x3 numpy 数组。 t_arm: 平移向量的 Python list, 每个元素是 3x1 numpy 数组。 """ R_arm = [] t_arm = [] for pose_matrix in self.robot_poses: # # 1. 提取旋转矩阵 (3x3) rotation_matrix = pose_matrix[:3, :3] R_arm.append(rotation_matrix) # R_arm.append(pose_matrix[:3]) # 2. 提取平移向量 (3x1) translation_vector = pose_matrix[:3, 3] # 获取齐次矩阵的平移部分 (1x3 numpy 数组) translation_vector = translation_vector.reshape(3, 1) # 转换为 3x1 的列向量 t_arm.append(translation_vector) return R_arm, t_arm def calibrate(self): rvecs, tvecs = self.camera_calib() r_arm, t_arm= self.split_robot_pose() rvecs_array = np.array(rvecs) tvecs_array = np.array(tvecs) t_arma = np.array(t_arm) r_arma = np.array(r_arm) print(rvecs_array.shape) print(tvecs_array.shape) print(t_arma.shape) print(r_arma.shape) R, t = cv2.calibrateHandEye(r_arma, t_arma, rvecs_array, tvecs_array, cv2.CALIB_HAND_EYE_TSAI) print("+++++++++++手眼标定完成+++++++++++++++") return R, t ,self.intrinsics_mtx #得到图片以后 根据pose判断方向： def get_next_pose_future(self): return None def get_next_pose_ugly(self,index): poselist =[ [-4.513801574707031250e+01,5.518265151977539062e+01,-1.237858963012695312e+02,9.085088729858398438e+00,-9.337700653076171875e+01,-9.000016784667968750e+01], [-4.594435882568359375e+01,3.636156082153320312e+01,-1.064173660278320312e+02,-2.125140428543090820e+00,-9.337675476074218750e+01,-9.000016784667968750e+01], [-4.595432662963867188e+01,1.059147930145263672e+01,-8.284246063232421875e+01,-1.484527587890625000e+01,-9.337491607666015625e+01,-9.000013732910156250e+01], [-4.638331604003906250e+01,-6.824899196624755859e+00,-6.758448791503906250e+01,-2.286166954040527344e+01,-9.337348937988281250e+01,-9.000016784667968750e+01], [-4.636801910400390625e+01,-1.807893753051757812e+01,-6.532709503173828125e+01,-2.261142921447753906e+01,-9.336659240722656250e+01,-9.000010681152343750e+01], [-4.637482833862304688e+01,-4.540402412414550781e+00,-1.098018112182617188e+02,3.086268234252929688e+01,-9.305430603027343750e+01,-9.000022125244140625e+01], [-4.640084075927734375e+01,8.068140029907226562e+00,-1.362033843994140625e+02,6.259086990356445312e+01,-9.334789276123046875e+01,-9.000019073486328125e+01], [-4.640056610107421875e+01,8.061657905578613281e+00,-1.511797027587890625e+02,8.927394104003906250e+01,-9.331819915771484375e+01,-9.000019073486328125e+01], [-4.638466262817382812e+01,3.287191867828369141e+00,-1.566032409667968750e+02,1.035896453857421875e+02,-9.330465698242187500e+01,-9.000022125244140625e+01], [-3.178509473800659180e+00,6.977911472320556641e+00,-1.417592010498046875e+02,6.005132293701171875e+01,-1.280102996826171875e+02,-9.000010681152343750e+01], [-1.424913024902343750e+01,-2.980865538120269775e-01,-1.193396759033203125e+02,3.752756500244140625e+01,-1.278784713745117188e+02,-8.999975585937500000e+01], [-2.393187522888183594e+01,-2.973175048828125000e-01,-1.121088027954101562e+02,3.928178024291992188e+01,-1.183067703247070312e+02,-8.999983215332031250e+01], [-3.699544525146484375e+01,-2.981689572334289551e-01,-1.078408889770507812e+02,3.178039932250976562e+01,-1.024006271362304688e+02,-8.999983215332031250e+01], [-6.734904479980468750e+01,-4.368630886077880859e+00,-1.035898971557617188e+02,2.342595481872558594e+01,-7.085527801513671875e+01,-8.999980926513671875e+01], [-8.632910919189453125e+01,-1.039798259735107422e+01,-1.038105545043945312e+02,2.330961036682128906e+01,-5.476583099365234375e+01,-8.999980926513671875e+01], [-8.156823730468750000e+01,5.878097534179687500e+00,-1.439221954345703125e+02,6.846669006347656250e+01,-4.482905197143554688e+01,-8.999922943115234375e+01], [-8.601764678955078125e+01,3.570293045043945312e+01,-1.308228912353515625e+02,2.318510818481445312e+01,-5.378013610839843750e+01,-8.999922943115234375e+01], [-7.132132053375244141e+00,3.722338104248046875e+01,-1.065378265380859375e+02,-3.337371826171875000e+00,-1.185582199096679688e+02,-8.999931335449218750e+01] ] return poselist[index] if __name__ == '__main__': # 标定板的width 对应的角点的个数 6 # 标定板的height 对应的角点的个数 3 calibration_board_size = [6,3] calibration_board_square_size = 0.03 #unit - meter systemPath = "/home/jsfb/Documents/" now = datetime.now() date_time_str = now.strftime("%d_%H_%M") folder_name = f"calibration_data_{date_time_str}" systemPath+=folder_name os.makedirs(systemPath, exist_ok=True) calib = Calibration(calibration_board_size,calibration_board_square_size,systemPath) calib.collect_data() print("=================数据采集完成===================") R, t,intrin = calib.calibrate() print("旋转矩阵：") print(R) print("平移向量：") print(t) print(f"内参： {intrin}")