用Python写一个手眼标定的代码

手眼标定（Hand-Eye Calibration）是指通过已知的手臂运动与相机运动的关系来求得相机的位姿。下面是一个用Python实现的手眼标定代码示例：

import numpy as np
from scipy.spatial.transform import Rotation

# 已知的手臂运动和相机运动的关系（手臂末端到相机坐标系的变换矩阵）
A = np.array([[0.707, -0.707, 0, 0.5],
              [0.707, 0.707, 0, -0.5],
              [0, 0, 1, 0.2],
              [0, 0, 0, 1]])

# 手臂末端的位姿
T = np.array([[0.707, -0.707, 0, 0.2],
              [0.707, 0.707, 0, 0.2],
              [0, 0, 1, 0.2],
              [0, 0, 0, 1]])

# 相机拍摄的物体位姿
B = np.array([[0.707, -0.707, 0, 0.1],
              [0.707, 0.707, 0, 0.1],
              [0, 0, 1, 0.3],
              [0, 0, 0, 1]])

# 手臂末端的位姿集合
T_set = np.array([T1, T2, T3, T4, T5])

# 相机拍摄的物体位姿集合
B_set = np.array([B1, B2, B3, B4, B5])

# 将矩阵展成向量
def mat_to_vec(T):
    return np.hstack((T[:3, :3].ravel(), T[:3, 3]))

# 将向量还原成矩阵
def vec_to_mat(v):
    return np.vstack((np.hstack((Rotation.from_matrix(v[:9].reshape(3, 3)).as_quat(), v[9:12])), [0, 0, 0, 1]))

# 计算相机运动的变换矩阵
def hand_eye_calibration(T_set, B_set):
    n = T_set.shape[0]
    A_list = []
    B_list = []
    for i in range(n):
        for j in range(i+1, n):
            Ti_Tj = np.linalg.inv(T_set[i]) @ T_set[j]
            Bi_Bj = np.linalg.inv(B_set[i]) @ B_set[j]
            A_list.append(mat_to_vec(Ti_Tj))
            B_list.append(mat_to_vec(Bi_Bj))
    A = np.vstack(A_list)
    B = np.vstack(B_list)
    X = np.linalg.inv(A.T @ A) @ A.T @ B
    return vec_to_mat(X)

# 进行手眼标定
X = hand_eye_calibration(T_set, B_set)
print(X)

上面的代码中，`A`表示已知的手臂运动和相机运动的关系，`T`表示手臂末端的位姿，`B`表示相机拍摄的物体位姿。`T_set`和`B_set`分别是多组手臂末端的位姿和相机拍摄的物体位姿，用于进行手眼标定。`mat_to_vec`和`vec_to_mat`分别是将矩阵展成向量和将向量还原成矩阵的函数。`hand_eye_calibration`是进行手眼标定的函数，它通过已知的手臂运动和相机运动的关系，求得相机的位姿。最后，调用`hand_eye_calibration`函数进行手眼标定，并输出相机的位姿。