双目相机手眼标定代码 Python

以下是双目相机手眼标定的 Python 代码示例：

import numpy as np
import cv2


def hand_eye_calibration(p_robot, p_camera, q_robot, q_camera):
    """
    双目相机手眼标定函数

    :param p_robot: 机器人运动轨迹中机械臂末端位姿的列表，每个位姿为一个 4x4 的齐次变换矩阵
    :param p_camera: 相机运动轨迹中相机位姿的列表，每个位姿为一个 4x4 的齐次变换矩阵
    :param q_robot: 机器人运动轨迹中机械臂末端位姿对应的机器人关节角度的列表
    :param q_camera: 相机运动轨迹中相机位姿对应的机器人关节角度的列表
    :return: 相机到机器人的变换矩阵 H, 以及机器人到相机的变换矩阵 G
    """
    n = len(p_robot)

    A = np.zeros((3*n, 3))
    B = np.zeros((3*n, 1))

    for i in range(n):
        Ap = np.eye(4) - p_robot[i]
        Aq = q_camera[i] - q_robot[i]
        Bp = p_camera[i] - p_robot[i]

        A[3*i:3*i+3, :] = np.dot(Ap[:3, :3], Aq.reshape(-1, 1)).T
        B[3*i:3*i+3, :] = np.dot(Ap[:3, :3], Bp[:3, 3].reshape(-1, 1)).T

    x = np.dot(np.linalg.pinv(A), B)

    H = np.eye(4)
    H[:3, :3] = x.reshape(3, 3)
    G = np.eye(4)
    G[:3, :3] = np.linalg.inv(x.reshape(3, 3))

    return H, G

其中，`p_robot` 和 `p_camera` 分别为机器人运动轨迹和相机运动轨迹中的位姿，每个位姿为一个 4x4 的齐次变换矩阵；`q_robot` 和 `q_camera` 分别为机器人运动轨迹和相机运动轨迹中的机器人关节角度，是一个列表。返回值为相机到机器人的变换矩阵 `H`，以及机器人到相机的变换矩阵 `G`。