update 修改姿态

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]  # 单位化方向向量

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)