添加 Trace/vec_change.py

Trace/vec_change.py

@@ -0,0 +1,112 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
+
+def plot_coordinate_system(ax, T, name, color, labels):
+    """绘制坐标系"""
+    origin = T[:3, 3]
+    x_axis = origin + T[:3, 0] * 300  # X 轴
+    y_axis = origin + T[:3, 1] * 300  # Y 轴
+    z_axis = origin + T[:3, 2] * 300  # Z 轴
+
+    # 绘制原点
+    ax.scatter(*origin, color=color, s=100)
+
+    # 绘制轴线
+    ax.quiver(*origin, *(x_axis - origin), color='r', length=1, arrow_length_ratio=0.2, linewidth=2)
+    ax.quiver(*origin, *(y_axis - origin), color='g', length=1, arrow_length_ratio=0.2, linewidth=2)
+    ax.quiver(*origin, *(z_axis - origin), color='b', length=1, arrow_length_ratio=0.2, linewidth=2)
+
+    # 标注坐标系名称
+    ax.text(*x_axis, f'{labels[0]}', color='r', fontsize=12)
+    ax.text(*y_axis, f'{labels[1]}', color='g', fontsize=12)
+    ax.text(*z_axis, f'{labels[2]}', color='b', fontsize=12)
+
+# A 到 B 的齐次转换矩阵 (工具到基坐标系)
+T_AB = np.array([[-9.36910568e-01,-4.37100341e-03, 3.49541818e-01, 5.04226000e+02],
+ [-5.82144893e-03, 9.99978253e-01, -3.09911034e-03, 2.62300000e+00],
+ [-3.49520671e-01, -4.93842907e-03, -9.36915638e-01, 5.23709000e+02],
+ [ 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 1.00000000e+00]])
+
+# B 到 C 的齐次转换矩阵 (相机到工具)
+T_BC = np.loadtxt('./cam_pose.txt', delimiter=' ')
+
+# 计算 A 到 C 的齐次转换矩阵
+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]
+])
+
+# 将角点从相机坐标系转换到法兰坐标系
+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))])
+
+# 计算每条边的长度
+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]  # 单位化方向向量
+
+# 假设法向量 (a, b, c) 在相机坐标系下
+normal_vector_camera = np.array([-0.1305,0.38402,0.91404, 0])  # 最后一个元素为0，因为它是方向矢量
+
+# 将法向量从相机坐标系转换到法兰坐标系
+normal_vector_flange = T_BC @ normal_vector_camera
+
+# 将法向量从法兰坐标系转换到基坐标系
+normal_vector_base = T_AB @ normal_vector_flange
+
+# 创建 3D 图形对象
+fig = plt.figure()
+ax = fig.add_subplot(111, projection='3d')
+
+# 设置绘图区域的范围
+ax.set_xlim([-1000, 1000])
+ax.set_ylim([-1000, 1000])
+ax.set_zlim([-1000, 1000])
+
+# 绘制基坐标系 O
+plot_coordinate_system(ax, np.eye(4), 'O', 'k', ['x', 'y', 'z'])
+
+# 绘制法兰坐标系 B
+plot_coordinate_system(ax, T_AB, 'B', 'm', ["x'", "y'", "z'"])
+
+# 绘制相机坐标系 C
+plot_coordinate_system(ax, T_AC, '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)
+
+# 绘制法向量 (基坐标系下)
+normal_vector_endpoint = normal_vector_base[: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)
+
+# 在基坐标系下绘制四个角点和边
+ax.scatter(corner_points_base[:, 0], corner_points_base[:, 1], corner_points_base[:, 2], color='b', s=50, label='Corners')
+for i in range(len(corner_points_base)):
+    ax.plot([corner_points_base[i - 1, 0], corner_points_base[i, 0]],
+            [corner_points_base[i - 1, 1], corner_points_base[i, 1]],
+            [corner_points_base[i - 1, 2], corner_points_base[i, 2]], 'k--')
+
+# 设置标签
+ax.set_xlabel('X')
+ax.set_ylabel('Y')
+ax.set_zlabel('Z')
+
+# 显示图形
+plt.show()