You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
25 lines
729 B
25 lines
729 B
3 months ago
|
|
||
|
|
|
||
|
|
import numpy as np
|
||
|
|
# 定义转换矩阵R_T
|
||
|
|
R_T = np.array([
|
||
|
|
[-0.7206, -0.6908, 0],
|
||
|
|
[0.6884, -0.7231, 0],
|
||
|
|
[0.0830, 0, 1]
|
||
|
|
])
|
||
|
|
|
||
|
|
|
||
|
|
def transform_data(x_coord, y_coord, z_coord):
|
||
|
|
# 将接收到的3个坐标组成列矩阵P_L1_O2
|
||
|
|
P_L1_O2 = np.array([[x_coord], [y_coord], [z_coord]], dtype=np.float64)
|
||
|
|
|
||
|
|
# 执行矩阵乘法：P_L1_O1 = R_T * P_L1_O2
|
||
|
|
P_L1_O1 = np.dot(R_T, P_L1_O2)
|
||
|
|
|
||
|
|
# 提取转换后的三个数据
|
||
|
|
increments_x = P_L1_O1[0][0]
|
||
|
|
increments_y = P_L1_O1[1][0]
|
||
|
|
increments_z = P_L1_O1[2][0]
|
||
|
|
|
||
|
|
#print(f"矩阵转换完成 - increments_x: {increments_x:.4f}, increments_y: {increments_y:.4f}, increments_z: {increments_z:.4f}")
|
||
|
|
return increments_x, increments_y, increments_z
|