From 0751e6cafdb0e1a33d974b52fec051134e228ed3 Mon Sep 17 00:00:00 2001 From: FrankCV2048 <1395405735@qq.com> Date: Fri, 13 Dec 2024 23:16:44 +0800 Subject: [PATCH] =?UTF-8?q?update=20=E4=BF=AE=E6=94=B9=E5=A7=BF=E6=80=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Trace/handeye_calibration.py | 4 +-- Trace/vec_change.py | 57 ++++++++++++++++++++---------------- Util/util_pic.py | 44 ++++++++++++++++------------ 3 files changed, 59 insertions(+), 46 deletions(-) diff --git a/Trace/handeye_calibration.py b/Trace/handeye_calibration.py index fb69af5..a224329 100644 --- a/Trace/handeye_calibration.py +++ b/Trace/handeye_calibration.py @@ -62,8 +62,8 @@ def getPosition(x,y,z,a,b,c,rotation,points): target_position = np.dot(camera2robot, target) corner_points_camera = np.asarray(points) - corner_points_base = np.dot(camera2robot[:3, :3], corner_points_camera.T).T + camera2robot[:3, 3] - edges = np.array([corner_points_base[i] - corner_points_base[i - 1] for i in range(len(corner_points_base))]) + corner_points_base = np.dot(T_BC[:3, :3], corner_points_camera.T).T + T_BC[:3, 3] + edges = np.array([corner_points_base[1] - corner_points_base[0]]) # for i in range(len(corner_points_base))]) edge_lengths = np.linalg.norm(edges, axis=1) min_edge_idx = np.argmin(edge_lengths) short_edge_direction = edges[min_edge_idx] / edge_lengths[min_edge_idx] # 单位化方向向量 diff --git a/Trace/vec_change.py b/Trace/vec_change.py index 3bd79cf..c53f2d3 100644 --- a/Trace/vec_change.py +++ b/Trace/vec_change.py @@ -29,43 +29,48 @@ T_AB = np.array([[-9.36910568e-01,-4.37100341e-03, 3.49541818e-01, 5.04226000e+0 [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]) # B 到 C 的齐次转换矩阵 (相机到工具) -T_BC = np.loadtxt('./cam_pose.txt', delimiter=' ') +T_BC = np.loadtxt('./com_pose.txt', delimiter=' ') # 计算 A 到 C 的齐次转换矩阵 -T_AC = T_AB @ T_BC +# T_AC = T_AB @ T_BC # 输入四个角点的空间坐标 (相机坐标系下) corner_points_camera = np.array([ - [-0.07010334103611927, -0.007624093814835717, 0.638128259308727], - [0.03136683809654154, 0.003582437967995489, 0.6478950644491274], - [0.02505911529466708, 0.04860494901872052, 0.625793874394482], - [-0.07246355234803807, 0.03687307395179221, 0.6171704935761987] +[-804.54114, -259.56854, 1764.0], +[-466.9239, -266.63162, 1812.0], +[-451.46283, 206.7851, 1752.0], +[-762.1249, 197.22481, 1671.0] ]) +# corner_points_camera = np.array([ +# [-0.07246355234803807, 0.03687307395179221, 0.6171704935761987], +# [0.02505911529466708, 0.04860494901872052, 0.625793874394482], +# [0.03136683809654154, 0.003582437967995489, 0.6478950644491274], +# [-0.07010334103611927, -0.007624093814835717, 0.638128259308727] +# +# ]) +# corner_points_camera = np.array([ +# [-0.07010334103611927, -0.007624093814835717, 0.638128259308727], +# [0.03136683809654154, 0.003582437967995489, 0.6478950644491274], +# [0.02505911529466708, 0.04860494901872052, 0.625793874394482], +# [-0.07246355234803807, 0.03687307395179221, 0.6171704935761987] +# ]) # 将角点从相机坐标系转换到法兰坐标系 -corner_points_flange = np.dot(T_BC[:3, :3], corner_points_camera.T).T + T_BC[:3, 3] -# 将角点从法兰坐标系转换到基坐标系 -corner_points_base = np.dot(T_AB[:3, :3], corner_points_flange.T).T + T_AB[:3, 3] - -# 计算每两个相邻角点之间的边向量 -edges = np.array([corner_points_base[i] - corner_points_base[i - 1] for i in range(len(corner_points_base))]) - -# 计算每条边的长度 +corner_points_base = np.dot(T_BC[:3, :3], corner_points_camera.T).T + T_BC[:3, 3] +edges = np.array([corner_points_base[1] - corner_points_base[0]])# for i in range(len(corner_points_base))]) edge_lengths = np.linalg.norm(edges, axis=1) - -# 找到最长的边 -max_edge_idx = np.argmax(edge_lengths) -long_edge_direction = edges[max_edge_idx] / edge_lengths[max_edge_idx] # 单位化方向向量 +min_edge_idx = np.argmin(edge_lengths) +short_edge_direction = edges[min_edge_idx] / edge_lengths[min_edge_idx] # 单位化方向向量 # 假设法向量 (a, b, c) 在相机坐标系下 -normal_vector_camera = np.array([-0.1305,0.38402,0.91404, 0]) # 最后一个元素为0，因为它是方向矢量 +normal_vector_camera = np.array([-0.23022874142597569, 0.12287340685252988, 0.965348047343477, 0]) # 最后一个元素为0，因为它是方向矢量 # 将法向量从相机坐标系转换到法兰坐标系 normal_vector_flange = T_BC @ normal_vector_camera # 将法向量从法兰坐标系转换到基坐标系 -normal_vector_base = T_AB @ normal_vector_flange +# normal_vector_base = T_AB @ normal_vector_flange # 创建 3D 图形对象 fig = plt.figure() @@ -80,19 +85,19 @@ ax.set_zlim([-1000, 1000]) plot_coordinate_system(ax, np.eye(4), 'O', 'k', ['x', 'y', 'z']) # 绘制法兰坐标系 B -plot_coordinate_system(ax, T_AB, 'B', 'm', ["x'", "y'", "z'"]) +# plot_coordinate_system(ax, T_AB, 'B', 'm', ["x'", "y'", "z'"]) # 绘制相机坐标系 C -plot_coordinate_system(ax, T_AC, 'C', 'b', ["x''", "y''", "z''"]) +plot_coordinate_system(ax, T_BC, 'C', 'b', ["x''", "y''", "z''"]) # 绘制长边方向向量 (基坐标系下) origin = np.zeros(3) # 基坐标系的原点 -long_edge_endpoint = long_edge_direction * 300 -ax.quiver(*origin, *(long_edge_endpoint), color='orange', length=1, arrow_length_ratio=0.2, linewidth=2) -ax.text(*long_edge_endpoint, 'Long Edge', color='orange', fontsize=12) +short_edge_endpoint = short_edge_direction * 300 +ax.quiver(*origin, *(short_edge_endpoint), color='orange', length=1, arrow_length_ratio=0.2, linewidth=2) +ax.text(*short_edge_endpoint, 'Short Edge', color='orange', fontsize=12) # 绘制法向量 (基坐标系下) -normal_vector_endpoint = normal_vector_base[:3] * 300 +normal_vector_endpoint = normal_vector_flange[:3] * 300 ax.quiver(*origin, *(normal_vector_endpoint), color='purple', length=1, arrow_length_ratio=0.2, linewidth=2) ax.text(*normal_vector_endpoint, 'Normal Vector', color='purple', fontsize=12) diff --git a/Util/util_pic.py b/Util/util_pic.py index e6ef291..ef39982 100644 --- a/Util/util_pic.py +++ b/Util/util_pic.py @@ -1,25 +1,33 @@ -import logging - -import PySide6 import cv2 -from PyQt5.QtGui import QImage, QPixmap - -from Util.util_log import log +from PIL.ImageQt import QImage, QPixmap def cv2_to_qpixmap(cv_img): """将OpenCV图像转换为QPixmap""" - if cv_img.shape !=3: - print("cv_img.shape !=3") - return None - try: - img = cv_img.copy() - height, width, channel = img.shape - bytes_per_line = 3 * width - q_img = QImage(img.data, width, height, bytes_per_line, QImage.Format_RGB888) - return QPixmap.fromImage(q_img) - except Exception as e: - log.log_message(logging.ERROR, e) - return None + height, width, channel = cv_img.shape + bytes_per_line = 3 * width + q_img = QImage(cv_img.data, width, height, bytes_per_line, QImage.Format_RGB888) + return QPixmap.fromImage(q_img) + +# def cv2_to_qpixmap(cv_img): +# """将OpenCV图像转换为QPixmap""" +# if len(cv_img.shape) !=3: +# print("cv_img.shape !=3") +# return None +# try: +# img = cv_img.copy() +# height, width, channel = img.shape +# bytes_per_line = 3 * width +# q_img = QImage(img.data, width, height, bytes_per_line, QImage.Format_RGB888) +# return QPixmap.fromImage(q_img) +# except Exception as e: +# log.log_message(logging.ERROR, e) +# return None +# def cv2_to_qpixmap(cv_img): +# #g = cv2.cvtColor(cv_img, cv2.COLOR_BGR2RGB) +# height, width, channel = cv_img.shape +# bytes_per_line = 3 * width +# q_img = QImage(cv_img.data, width, height, bytes_per_line, QImage.Format_RGB888) +# return QPixmap.fromImage(q_img)