135 lines
4.9 KiB
Python
135 lines
4.9 KiB
Python
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('./com_pose.txt', delimiter=' ')
|
||
|
||
# 计算 A 到 C 的齐次转换矩阵
|
||
# T_AC = T_AB @ T_BC
|
||
|
||
# 输入四个角点的空间坐标 (相机坐标系下)
|
||
# corner_points_camera = np.array([
|
||
# [-605.3829, 288.2771, 1710.0],
|
||
# [-364.94568, 300.40274, 1634.0],
|
||
# [-301.4996, -253.04178, 1645.0],
|
||
# [-548.8065, -297.23093, 1748.0]
|
||
# ])
|
||
#
|
||
# # 将角点从相机坐标系转换到基坐标系
|
||
#
|
||
# 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] # 单位化方向向量
|
||
|
||
|
||
corner_points_camera = np.array([
|
||
[-548.8065, -297.23093, 1748.0],
|
||
[-301.4996, -253.04178, 1645.0],
|
||
[-364.94568, 300.40274, 1634.0],
|
||
[-605.3829, 288.2771, 1710.0]
|
||
])
|
||
|
||
# 将角点从相机坐标系转换到基坐标系
|
||
corner_points_base = np.dot(T_BC[:3, :3], corner_points_camera.T).T + T_BC[:3, 3]
|
||
|
||
# 按照 x 轴排序
|
||
sorted_points = corner_points_base[np.argsort(corner_points_base[:, 0])]
|
||
|
||
# 选出x轴较大的两个点
|
||
point_1 = sorted_points[-1] # x值较大的点
|
||
point_2 = sorted_points[-2] # x值较小的点
|
||
|
||
# 根据 y 值选择差值方向，y值较大的点减去 y 值较小的点
|
||
if point_1[1] > point_2[1]:
|
||
edge_vector = point_1 - point_2
|
||
else:
|
||
edge_vector = point_2 - point_1
|
||
|
||
# 单位化方向向量
|
||
short_edge_direction = edge_vector / np.linalg.norm(edge_vector)
|
||
|
||
print("方向向量（单位化）：", short_edge_direction)
|
||
|
||
|
||
# 假设法向量 (a, b, c) 在相机坐标系下
|
||
normal_vector_camera = np.array([0.2694268969253701, 0.033645691818738714, 0.9624329143556991, 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_BC, 'C', 'b', ["x''", "y''", "z''"])
|
||
|
||
# 绘制长边方向向量 (基坐标系下)
|
||
origin = np.zeros(3) # 基坐标系的原点
|
||
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_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)
|
||
|
||
# 在基坐标系下绘制四个角点和边
|
||
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()
|