293 lines
14 KiB
Python
293 lines
14 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_aubo/Massage/MassageControl')
|
||
from hardware.remote_cam import ToolCamera
|
||
from hardware.aubo_C5 import AuboC5
|
||
|
||
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.objp = self._create_object_points()
|
||
self.image_points = []
|
||
self.robot_poses = []
|
||
self.intrinsics = None
|
||
self.distortion_cofficients = None
|
||
self.cam = ToolCamera(host='127.0.0.1')
|
||
self.cam.start()
|
||
time.sleep(5)
|
||
self.size = (400,640)
|
||
rgb_image, depth_image, camera_intrinsics = self.cam.get_latest_frame()
|
||
if rgb_image is None :
|
||
print(f'===============相机连接失败请检查并重试==============')
|
||
sys.exit(1)
|
||
else:
|
||
print(f'===============相机连接成功==============')
|
||
self.size = rgb_image.shape[:2]
|
||
self.intrinsics = camera_intrinsics
|
||
|
||
self.arm = AuboC5()
|
||
self.arm.init()
|
||
self.obj_points = []
|
||
print(f'===============机械臂连接成功==============')
|
||
date_time_str = time.strftime("%Y%m%d%H%M%S", time.localtime())
|
||
folder_name = f"calibration_data_{date_time_str}"
|
||
file_folder+=folder_name
|
||
os.makedirs(file_folder, exist_ok=True)
|
||
print(f'===============创建文件夹成功=============')
|
||
self.file_address = file_folder
|
||
|
||
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 = [(90) * (np.pi / 180), 4.23 * (np.pi / 180), 124.33 * (np.pi / 180), 61.56 * (np.pi / 180), -89.61 * (np.pi / 180), 0 * (np.pi / 180)]
|
||
code = self.arm.move_joint(pose,4,wait=True)
|
||
if code == -1:
|
||
print("运动到拍照位置失败")
|
||
#拍照
|
||
self.check_corners()
|
||
#运动到下一个位置
|
||
# 目前只设计了44条轨迹
|
||
for i in range (0,45):
|
||
next_pose = self.get_next_pose_ugly(i)
|
||
self.arm.move_joint(next_pose, 4,wait=True)
|
||
if code == -1:
|
||
print("运动到拍照位置失败")
|
||
else:
|
||
self.check_corners()
|
||
|
||
def check_corners(self):
|
||
rgb, depth, intrinsics = self.cam.get_latest_frame()
|
||
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):
|
||
pose = self.arm.robot_rpc_client.getRobotInterface(self.arm.robot_name).getRobotState().getTcpPose()
|
||
x, y, z, rx, ry, rz = pose
|
||
r = R.from_euler('xyz', [rx, ry, rz])
|
||
rotation_matrix = r.as_matrix()
|
||
translation_vector = np.array([x, y, z]).reshape(3, 1)
|
||
homo_mat = np.eye(4) # 创建 4x4 单位矩阵
|
||
homo_mat[:3, :3] = rotation_matrix
|
||
homo_mat[:3, 3] = translation_vector.flatten()
|
||
return homo_mat
|
||
|
||
def camera_calib(self):
|
||
# print(f"\n\n\n\n\nsize 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("++++++++++相机标定完成++++++++++++++")
|
||
# 从mtx中提取内参
|
||
fx = mtx[0, 0] # 焦距 fx
|
||
fy = mtx[1, 1] # 焦距 fy
|
||
cx = mtx[0, 2] # 主点 cx
|
||
cy = mtx[1, 2] # 主点 cy
|
||
|
||
# 从 distortion_array 提取畸变系数
|
||
k1 = dist[0][0] # 畸变系数 k1
|
||
k2 = dist[0][1] # 畸变系数 k2
|
||
p1 = dist[0][2] # 畸变系数 p1
|
||
p2 = dist[0][3] # 畸变系数 p2
|
||
k3 = dist[0][4] # 畸变系数 k3
|
||
# self.intrinsics = mtx
|
||
self.intrinsics = {
|
||
'fx': fx,
|
||
'fy': fy,
|
||
'cx': cx,
|
||
'cy': cy,
|
||
'distortion_coeffs': {
|
||
'k1': k1,
|
||
'k2': k2,
|
||
'p1': p1,
|
||
'p2': p2,
|
||
'k3': k3
|
||
}
|
||
}
|
||
self.distortion_cofficients = 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()
|
||
print(rvecs)
|
||
print(tvecs)
|
||
print(r_arm)
|
||
print(t_arm)
|
||
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("+++++++++++手眼标定完成+++++++++++++++")
|
||
self.arm.exit_task()
|
||
return R, t ,self.intrinsics
|
||
|
||
#得到图片以后 根据pose判断方向：
|
||
def get_next_pose_future(self):
|
||
return None
|
||
def get_next_pose_ugly(self,index):
|
||
poselist =[
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.5116296651522887, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.4592697875924587, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0220648099678793, -1.5116296651522887, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9697049324080494, -1.4592697875924587, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9173450548482196, -1.4069099100326288, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.6163494202719486, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.6687092978317786, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.2315043202071991, -1.7210691753916085, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.4941763726323454, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.424363202552572, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.3545500324727988, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.2140510276872558, -1.2847368623930255, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.633802712791892, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.7036158828716652, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 0.9347983473681627, -1.2847368623930255, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
|
||
[1.5707963267948966, 0.06510078109938851, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0],
|
||
[1.5707963267948966, 0.05637413483941686, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0],
|
||
[1.5707963267948966, 0.047647488579445216, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
|
||
[1.562069680534925, 0.07382742735936015, 2.16996785900455, 1.0395181024878226, -1.4941763726323454, 0.0],
|
||
[1.5533430342749535, 0.07382742735936015, 2.16996785900455, 1.004611517447936, -1.424363202552572, 0.0],
|
||
[1.544616388014982, 0.07382742735936015, 2.16996785900455, 0.9697049324080493, -1.3545500324727988, 0.0],
|
||
[1.5358897417550104, 0.07382742735936015, 2.16996785900455, 0.9347983473681627, -1.2847368623930255, 0.0],
|
||
[1.5271630954950388, 0.07382742735936015, 2.16996785900455, 0.8998917623282761, -1.2149236923132523, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
|
||
[1.579522973054868, 0.07382742735936015, 2.16996785900455, 1.109331272567596, -1.5988961277520053, 0.0],
|
||
[1.5882496193148397, 0.07382742735936015, 2.16996785900455, 1.1442378576074825, -1.633802712791892, 0.0],
|
||
[1.5969762655748112, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.6687092978317786, 0.0],
|
||
[1.6057029118347828, 0.07382742735936015, 2.16996785900455, 1.2140510276872558, -1.7036158828716652, 0.0],
|
||
[1.6144295580947543, 0.07382742735936015, 2.16996785900455, 1.2489576127271425, -1.7385224679115518, 0.0],
|
||
[1.5707963267948966, 0.07382742735936015, 2.16996785900455, 1.0744246875277093, -1.5639895427121187, 0.0],
|
||
[1.579522973054868, 0.07382742735936015, 2.16996785900455, 1.1267845650875392, -1.581442835232062, 0.0],
|
||
[1.5882496193148397, 0.07382742735936015, 2.16996785900455, 1.1791444426473692, -1.5988961277520053, 0.0],
|
||
[1.5969762655748112, 0.07382742735936015, 2.16996785900455, 1.2315043202071991, -1.6163494202719486, 0.0],
|
||
[1.6057029118347828, 0.07382742735936015, 2.16996785900455, 1.283864197767029, -1.633802712791892, 0.0],
|
||
[1.6144295580947543, 0.07382742735936015, 2.16996785900455, 1.336224075326859, -1.6512560053118353, 0.0]
|
||
]
|
||
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/"
|
||
|
||
|
||
calib = Calibration(calibration_board_size,calibration_board_square_size,systemPath)
|
||
calib.collect_data()
|
||
R, t,intrinsics = calib.calibrate()
|
||
print("旋转矩阵：")
|
||
print(R)
|
||
print("平移向量：")
|
||
print(t)
|
||
print("内参：")
|
||
print(intrinsics) |