更新 Trace/handeye_calibration

Trace/handeye_calibration

@@ -1,4 +1,50 @@
 import numpy as np
+from scipy.spatial.transform import Rotation as R
+
+
+def flip_coefficient_if_positive(coefficient):
+    # 检查 coefficient[2] 是否大于0
+    if coefficient[2] > 0:
+        # 取反所有分量
+        coefficient = [-x for x in coefficient]
+        print("翻转：\n")
+
+    return coefficient
+
+def vec2ola(coefficient):
+    coefficient_raw = flip_coefficient_if_positive(coefficient)
+    coefficient = coefficient_raw.reshape(-1)
+    curZ = np.array(coefficient)# 定义 Z 方向的向量
+    # print(curZ)
+
+    curX = np.array([1, 1, 0],dtype=np.float64)# 定义初始 X 方向的向量
+    curX /= np.linalg.norm(curX)  # 归一化 X 向量
+
+    curY = np.cross(curZ, curX)# 计算 Y 方向的向量
+    curY /= np.linalg.norm(curY)  # 归一化 Y 向量
+
+    curX = np.cross(curY, curZ)# 重新计算 X 方向的向量
+    curX /= np.linalg.norm(curX)  # 归一化 X 向量
+
+    # 创建旋转矩阵
+    rotM = np.array([
+        [curX[0], curY[0], curZ[0]],
+        [curX[1], curY[1], curZ[1]],
+        [curX[2], curY[2], curZ[2]]
+    ])
+
+    # 打印旋转矩阵
+    print("Rotation Matrix:")
+    print(rotM)
+
+    # 计算欧拉角（XYZ顺序）并转换为度
+    r = R.from_matrix(rotM)
+    euler_angles = r.as_euler('xyz', degrees=True)
+
+    print("Euler Angles (degrees):")
+    print(euler_angles)
+
+
 
 def vec2attitude(a,b,c):
     pi = np.arccos(-1.0)  # pi 的值
@@ -23,13 +69,14 @@ def vec2attitude(a,b,c):
     matrix_result = np.dot(np.dot(np.dot(r1, r2), r3), vector1)
 
     # 输出结果
-    rpy=[]
+    ola=[]
     for m in range(matrix_result.shape[0]):
-        rpy.append(matrix_result[m][0])
+        ola.append(matrix_result[m][0])
         print(f"{matrix_result[m][0]:<20}")
 
-    return rpy
+    return ola
 
+#这个版本是先拿法向量转换成基坐标，在转欧拉
 #黄老师会给我传目标物的中心点坐标x,y,z和目标位姿的平面法向量a,b,c
 def getPosition(x,y,z,a,b,c):
     target = np.asarray([x, y, z])
@@ -37,16 +84,15 @@ def getPosition(x,y,z,a,b,c):
     position = np.dot(camera2robot[0:3, 0:3], target) + camera2robot[0:3, 3:]
     target_position = position[0:3, 0]#转换后的位置信息
 
-    # vector = np.asarray([a, b, c])
-    # normal_vector = vector / np.linalg.norm(vector)#归一化
-    # normal_vector.shape = (3, 1)
-    # dot_angle = np.dot(camera2robot[0:3, 0:3], normal_vector)#转换后的法向量,方向依然是同一个方向，只是表示方法变了
+    vector = np.asarray([a, b, c])
+    normal_vector = vector / np.linalg.norm(vector)#归一化
+    normal_vector.shape = (3, 1)
+    dot_angle = np.dot(camera2robot[0:3, 0:3], normal_vector)#转换后的法向量,方向依然是同一个方向，只是表示方法变了
 
-    # angle_tool = R.from_matrix(camera2robot[0:3, 0:3])
-    rpy = vec2attitude(a,b,c)
-    r,p,y=rpy[0],rpy[1],rpy[2]#r表示u,p表示v，y表示w
-    target_angle_raw = np.asarray([r,p,y])
-    target_angle = np.dot(camera2robot[0:3, 0:3],target_angle_raw)
-    print(target_position,target_angle)
+    target_angle = vec2ola(dot_angle)#把转换之后的法向量转换为欧拉角,欧拉角不是rpy角
 
-    return target_position, target_angle
+    # r,p,y = angle_tool.as_euler('xyz',degrees=True)#r表示u,p表示v，y表示w
+    # target_angle = np.asarray([r,p,y])
+    # print(target_angle)
+
+    return target_position,target_angle