243 lines
11 KiB
Python
243 lines
11 KiB
Python
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}")
|