双目相机的手眼标定代码（眼在手外）

双目相机的手眼标定可以分为两个步骤：手眼标定和相机标定。下面是一个示例代码，演示如何进行手眼标定（假设眼在手外）。

import numpy as np
import cv2

# 相机参数
K1 = np.array([...])  # 左相机内参矩阵
K2 = np.array([...])  # 右相机内参矩阵
D1 = np.array([...])  # 左相机畸变系数
D2 = np.array([...])  # 右相机畸变系数
R = np.array([...])   # 右相机到左相机的旋转矩阵
T = np.array([...])   # 右相机到左相机的平移向量

# 手眼标定参数
num_poses = 10  # 采集的手眼位姿数量
pose_list = []  # 手眼位姿列表，每个元素为 (R_hand, t_hand, R_eye, t_eye)
for i in range(num_poses):
    # 从手臂运动学获取当前手臂末端执行器姿态 R_hand 和位置 t_hand
    # 从外部传感器获取当前眼睛朝向姿态 R_eye 和位置 t_eye
    pose_list.append((R_hand, t_hand, R_eye, t_eye))

# 构造 A 矩阵和 B 矩阵
A = np.zeros((3 * num_poses, 3))
B = np.zeros((3 * num_poses, 1))
for i in range(num_poses):
    R_hand = pose_list[i][0]
    t_hand = pose_list[i][1]
    R_eye = pose_list[i][2]
    t_eye = pose_list[i][3]
    A[3*i:3*i+3, :] = np.eye(3) - R_hand @ R_eye
    B[3*i:3*i+3, :] = R_hand @ t_eye - t_hand

# 解方程 Ax = B，得到外参矩阵 T_hand_eye
T_hand_eye, _, _, _ = np.linalg.lstsq(A, B, rcond=None)

# 打印结果
print("Hand-eye calibration result:")
print(T_hand_eye)

这段代码首先定义了左右相机的内参矩阵、畸变系数以及右相机到左相机的旋转矩阵和平移向量。然后，通过手臂运动学和外部传感器获取了多组手眼位姿，构造了 A 矩阵和 B 矩阵，并通过最小二乘法求解出外参矩阵 T_hand_eye。最后，打印出了手眼标定结果。