相机不动，机构动作----Hands Eyes

最近在研究手眼标定，发现大家都需付费，搞啥子，说好的开源。。。

以相机在上固定不动，机械手为 EPSON_Robot 为例，详细的一步一步实例操作指引 EPSON_Robot 的192.168.0.1 2004 Server

详细操作步骤

1. 启动程序

运行以下 Python 代码后，会弹出一个标题为 "手眼标定操作指引" 的 UI 窗口，窗口内顶部首先会显示一个文本标签提示 "当前步骤：0/5"，代表当前处于整个操作流程的第 0 步，预估总共有 5 步操作，下方会看到左右排列的 "实时标定" 和 "图像标定" 两个按钮，以及下方的 "采集手眼数据"、"执行手眼标定"、"显示采集的数据" 以及 "重置操作" 四个按钮（后四个按钮初始时部分功能未绑定，等待选择标定模式后确定具体功能）。

2. 选择标定模式（以下分两种情况说明）

实时标定模式选择及操作：

点击 "实时标定" 按钮，此时会先重置所有相关变量和操作步骤状态（通过调用 reset_operation 函数实现），并将标定模式设置为 "实时标定"，然后 "采集手眼数据" 按钮绑定的功能会变为 collect_data_real_time 函数，"执行手眼标定" 按钮绑定的功能会变为 perform_calibration_real_time 函数。

图像标定模式选择及操作：

点击 "图像标定" 按钮，同样先重置相关状态并设置标定模式为 "图像标定"，"采集手眼数据" 按钮绑定的功能会变为 collect_data_image 函数，"执行手眼标定" 按钮绑定的功能会变为 perform_calibration_image 函数。

3. 实时标定模式下的具体操作步骤（假设已选择 "实时标定"）

采集第一组数据：
- 点击 "采集手眼数据" 按钮，此时会有以下操作和提示：
  - 首先弹出操作提示框显示 "接下来需要获取 EPSON_Robot 机器人手（末端执行器）的位姿信息，当前代码中只是简单模拟生成了一个单位矩阵（np.eye (4)）来表示手的位姿，在实际应用中，你需要通过和 EPSON_Robot 的控制系统进行通信来获取真实的末端执行器的位姿信息，这个位姿信息通常以齐次变换矩阵（4x4）的形式表示，包含了旋转和平移信息，并将其添加到 hand_poses 列表中。"，这里你需要按照 EPSON_Robot 提供的方式（比如通过网络通信、专用 API 等）获取真实的机器人末端位姿来替换模拟代码部分（后续步骤同此情况，暂用模拟数据示意流程）。
  - 然后又会弹出操作提示框显示 "接下来将尝试打开默认的摄像头获取图像中的棋盘格角点坐标，程序会先打开摄像头读取一帧图像，转换为灰度图像后进行角点检测。如果检测到了符合设定棋盘格尺寸的棋盘格，会将角点坐标等相关数据添加；若未检测到或摄像头打开失败会弹出相应提示，请根据提示进行操作。"，程序会尝试打开摄像头，读取一帧图像并进行处理检测棋盘格角点。
  - 如果摄像头成功打开且检测到了符合设定棋盘格尺寸（代码中设定为 board_size=(8, 6)）的棋盘格，会将检测到的棋盘格角点坐标（以像素为单位，格式为 Nx2 的 numpy 数组，N 为角点数量）添加到 image_points 列表中，同时对应的真实世界中的棋盘格角点坐标（在 objp 中定义，根据设定的棋盘格方格边长 square_size = 0.025 米和棋盘格布局生成的三维坐标）添加到 object_points 列表中，并且弹出提示框告知 "已成功采集一组手眼数据！"，同时会在一个300 * 300大小且居中的窗口显示当前实时图像 1 秒钟左右（方便你查看图像及棋盘格情况），便于确认采集的数据对应的图像情况。若未检测到棋盘格，会弹出提示框提示 "未检测到棋盘格，请调整相机视角后重新采集。"；若无法打开摄像头，则会弹出提示框提示 "无法打开摄像头，请检查设备连接。"。
采集多组数据 ：

重复点击 "采集手眼数据" 按钮，按照上述采集第一组数据的流程，操作 EPSON_Robot 使其末端执行器处于不同的位姿（比如通过编程控制机器人运动到不同位置、不同姿态角度等，具体操作依赖 EPSON_Robot 的操作手册和编程接口），同时确保相机能正常拍摄到棋盘格，每次成功采集都会更新 UI 上的步骤文本（如第二次点击变为 "当前步骤：2/5" 等），建议至少采集 4 组不同状态下的数据，确保机器人手和相机相对位置及姿态有足够的变化范围，这样可以提高标定的准确性。
执行标定 ：

当采集的数据组数满足至少 4 组后，点击 "执行手眼标定" 按钮，此时会发生以下操作及提示：
- 首先 current_step 变量会再次自增 1，UI 上的步骤文本更新为对应的数字（例如采集了 4 次数据后点击此按钮，就变为 "当前步骤：5/5"），代表进入到执行标定这一关键步骤，同时会弹出操作提示框显示 "首先会检查采集的数据数量是否足够用于标定，需要至少采集 4 组数据，如果不足会弹出错误提示框并终止标定操作，请确保之前已采集足够的数据。" 用于提醒你确认数据数量情况。
- 程序会检查采集的数据数量，如果不足会弹出错误提示框提示 "采集的数据不足，请至少采集 4 组数据进行标定！" 并终止标定操作，同时步骤文本也停留在当前数字不再变化。
- 如果数据足够，会弹出操作提示框显示 "接下来将调用 cv2.calibrateHandEye 函数进行手眼标定，采用的是 Tsai-Lenz 算法，该函数需要传入机器人手的旋转矩阵列表（从 hand_poses 中提取每个位姿的前 3x3 部分表示旋转）、平移向量列表（提取每个位姿的前 3 列第 4 行表示平移）、图像中棋盘格角点坐标列表（转换格式并转换数据类型为合适的 numpy 数组形式）以及真实世界中的棋盘格角点坐标列表（object_points）作为参数。" 来告知你标定算法及参数传入相关操作，然后调用 cv2.calibrateHandEye 函数进行手眼标定。
- 如果标定过程顺利，会将得到的旋转向量通过 cv2.Rodrigues 函数转换为旋转矩阵，并与平移向量组合成一个 4x4 的齐次变换矩阵形式存储在 calibration_result 变量中，表示手眼之间的坐标变换关系，最后弹出提示框展示这个标定得到的变换矩阵内容；若标定过程出现错误（例如数据格式不正确、算法计算出现异常等情况），会弹出错误提示框显示具体的错误信息，步骤文本同样停留在当前执行标定的这一步骤数字上。

4. 图像标定模式下的具体操作步骤（假设已选择 "图像标定"）

采集第一组数据：
- 点击 "采集手眼数据" 按钮，会弹出操作提示框显示 "接下来将生成一个模拟的棋盘格图像作为示例图像用于采集数据，程序会自动创建并保存图像文件到 C:\BD.png，然后提取棋盘格角点坐标以及对应的机器人手位姿信息（这里假设你已经有相关对应关系的数据，实际需完善获取及关联逻辑），然后添加到相应列表中用于后续标定。"。
- 程序会自动生成一个简单的 4 * 4 棋盘格样式的图像（以白色方格和黑色背景组成，模拟棋盘格），并将其存储在 C:\BD.png，然后在一个 300 * 300 大小且居中的窗口显示该图像 1 秒钟左右（方便你查看图像情况），之后模拟添加一组数据（这里暂时只是简单模拟添加，实际需要替换为真实从图像读取并关联对应机器人手位姿等逻辑），即添加一个模拟的机器人手位姿（np.eye(4) 模拟，实际要替换）到 hand_poses 列表，模拟的图像中的角点坐标（corners = np.array([[100, 200], [110, 210], [120, 220], [130, 230]])，实际要替换为真实从图像读取检测到的角点）到 image_points 列表，以及对应的真实世界中的棋盘格角点坐标（objp[0]）到 object_points 列表，最后弹出提示框告知 "已成功采集一组手眼数据（从图像文件）！"。
采集多组数据 ：

重复点击 "采集手眼数据" 按钮，按照上述采集第一组数据的流程，每次点击都会模拟添加一组数据（实际需要你准备多组包含棋盘格的图像文件，并且有对应的机器人手位姿信息，通过相应代码逻辑读取图像、提取角点坐标以及关联正确的手位姿添加到对应列表中），每次成功采集都会更新 UI 上的步骤文本（如第二次点击变为 "当前步骤：2/5" 等），建议至少采集 4 组不同状态下的数据，确保机器人手和相机相对位置及姿态有足够的变化范围（这里体现为不同图像中棋盘格与模拟的机器人手位姿的对应关系多样性），这样可以提高标定的准确性。
执行标定 ：

当采集的数据组数满足至少 4 组后，点击 "执行手眼标定" 按钮，操作流程和提示信息基本与实时标定模式下执行标定类似，会有步骤更新提示、数据数量检查、标定算法及参数传入提示等，若数据足够会进行标定计算，成功则展示标定结果，出现错误则弹出相应错误提示框，步骤文本也会根据操作状态停留在相应数字上。

5. 查看采集的数据（可选步骤，两种标定模式通用）

在采集了一定组数的数据后，你可以点击 "显示采集的数据" 按钮，会弹出一个新的提示框，里面展示了已采集的数据组数、手的位姿示例（展示第一个采集的手位姿矩阵）以及图像点坐标示例（展示第一个采集的图像中棋盘格角点坐标数组）等信息，方便你确认采集的数据是否合理以及是否符合预期，此操作步骤文本不会改变。

6. 重置操作（可选步骤，两种标定模式通用）

如果想要重新开始手眼标定流程，例如采集的数据不理想或者想要重新进行整个操作演示等情况，可以点击 "重置操作" 按钮，程序会执行以下操作：

内部会将所有相关的全局变量（hand_poses、image_points、object_points、calibration_result 以及 current_step）恢复到初始状态，清空之前采集的数据以及重置步骤计数为 0，然后通过 update_step_text 函数将 UI 上的步骤文本更新回 "当前步骤：0/5"，同时弹出提示框告知 "已重置操作，可重新开始手眼标定流程。"，之后就可以再次按照上述步骤重新进行手眼标定操作了。

python 复制代码

import cv2
import numpy as np
import tkinter as tk
from tkinter import messagebox, Label, Button
import socket
import struct
import os
import glob


# 全局变量
hand_poses = []  # 存储机器人手（末端执行器）的位姿，以齐次变换矩阵形式（4x4）
image_points = []  # 存储图像中对应的棋盘格角点坐标（Nx2，N为角点数量）
object_points = []  # 存储真实世界中的棋盘格角点坐标（Nx3）
calibration_result = None  # 存储标定结果
current_step = 0  # 当前操作步骤计数
total_steps = 5  # 预估的总操作步骤数（可根据实际情况调整）

# 棋盘格尺寸（内角点数，例如8x6）
board_size = (8, 6)
square_size = 0.025  # 棋盘格方格边长，单位米（根据实际情况设置）

# 生成真实世界中的棋盘格角点坐标
objp = np.zeros((board_size[0] * board_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:board_size[0], 0:board_size[1]].T.reshape(-1, 2) * square_size
objp = [objp]


def update_step_text():
    """更新UI上显示的步骤文本"""
    step_text.set(f"当前步骤：{current_step}/{total_steps}")


# 通过Socket通信获取EPSON_Robot机器人手位姿信息（根据给定协议模拟，实际需按详细规范调整）
def get_robot_pose_via_socket():
    """
    通过Socket通信按照指定的IP、端口以及指令格式获取机器人末端执行器位姿信息
    """
    try:
        # 创建Socket连接
        client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        client_socket.connect(("192.168.0.1", 2004))

        # 发送获取位姿数据的指令（按照给定格式）
        request_command = "Print #205, \"send data to pc\""
        client_socket.send(request_command.encode())

        # 接收数据（假设返回的数据格式是按一定顺序排列的浮点数表示4x4齐次变换矩阵，这里按此模拟解析，实际需按准确协议）
        received_data = client_socket.recv(1024)
        # 这里假设接收到的数据直接就是16个连续的浮点数表示位姿矩阵元素，实际可能需要更复杂的解析
        pose_data = struct.unpack('16f', received_data)
        pose_matrix = np.array(pose_data).reshape(4, 4)

        client_socket.close()
        return pose_matrix
    except socket.error as e:
        messagebox.showerror("Socket通信错误", f"Socket通信出现错误: {str(e)}，请检查网络连接及机器人配置。")
        return np.eye(4)  # 如果出现错误，返回单位矩阵作为占位（可根据实际情况调整错误处理方式）


# 采集数据函数（用于实时标定）
def collect_data_real_time():
    """
    实时采集手眼数据，包括获取EPSON_Robot机器人手的位姿和对应的图像中棋盘格角点坐标
    """
    global current_step
    current_step += 1
    update_step_text()
    # 通过Socket通信获取机器人手位姿
    hand_pose = get_robot_pose_via_socket()
    hand_poses.append(hand_pose)

    # 提醒获取图像中棋盘格角点坐标相关操作
    messagebox.showinfo("操作提示", "接下来将尝试打开默认的摄像头获取图像中的棋盘格角点坐标，程序会先打开摄像头读取一帧图像，转换为灰度图像后进行角点检测。如果检测到了符合设定棋盘格尺寸的棋盘格，会将角点坐标等相关数据添加；若未检测到或摄像头打开失败会弹出相应提示，请根据提示进行操作。")
    # 打开摄像头获取图像中的棋盘格角点坐标
    cap = cv2.VideoCapture(0)
    found, frame = cap.read()
    if found:
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        ret, corners = cv2.findChessboardCorners(gray, board_size, None)
        if ret:
            image_points.append(corners)
            object_points.append(objp[0])
            messagebox.showinfo("提示", "已成功采集一组手眼数据！")
            # 显示实时图像在300*300窗口且居中
            display_image(frame)
        else:
            messagebox.showerror("错误", "未检测到棋盘格，请调整相机视角后重新采集。")
    else:
        messagebox.showerror("错误", "无法打开摄像头，请检查设备连接。")
    cap.release()


def display_image(frame):
    """在300*300窗口且居中显示图像"""
    height, width = frame.shape[:2]
    max_dim = max(height, width)
    scale = 300 / max_dim
    new_width = int(width * scale)
    new_height = int(height * scale)
    resized_frame = cv2.resize(frame, (new_width, new_height))
    top = (300 - new_height) // 2
    left = (300 - new_width) // 2
    centered_frame = np.zeros((300, 300, 3), dtype=frame.dtype)
    centered_frame[top:top + new_height, left:left + new_width] = resized_frame
    cv2.imshow('实时图像', centered_frame)
    cv2.waitKey(1000)  # 显示1秒，可根据需要调整时间
    cv2.destroyAllWindows()


# 执行实时标定函数
def perform_calibration_real_time():
    """
    执行实时手眼标定操作，利用采集的数据计算手眼之间的变换关系
    """
    global current_step, calibration_result
    current_step += 1
    update_step_text()
    # 提醒检查数据数量是否足够
    messagebox.showinfo("操作提示", "首先会检查采集的数据数量是否足够用于标定，需要至少采集4组数据，如果不足会弹出错误提示框并终止标定操作，请确保之前已采集足够的数据。")
    if len(hand_poses) < 4 or len(image_points) < 4:
        messagebox.showerror("错误", "采集的数据不足，请至少采集4组数据进行标定！")
        return
    try:
        # 使用OpenCV的手眼标定函数（这里以Tsai-Lenz算法为例，实际可能有多种选择）
        # 提醒关于标定算法及参数传入相关操作
        messagebox.showinfo("操作提示", "接下来将调用cv2.calibrateHandEye函数进行手眼标定，采用的是Tsai-Lenz算法，该函数需要传入机器人手的旋转矩阵列表（从hand_poses中提取每个位姿的前3x3部分表示旋转）、平移向量列表（提取每个位姿的前3列第4行表示平移）、图像中棋盘格角点坐标列表（转换格式并转换数据类型为合适的numpy数组形式）以及真实世界中的棋盘格角点坐标列表（object_points）作为参数。")
        retval, rotation_vector, translation_vector = cv2.calibrateHandEye(
            [pose[:3, :3] for pose in hand_poses],
            [pose[:3, 3] for pose in hand_poses],
            [points.reshape(-1, 2).astype(np.float64) for points in image_points],
            object_points
        )

        # 将旋转向量和平移向量转换为变换矩阵形式方便后续使用
        rotation_matrix, _ = cv2.Rodrigues(rotation_vector)
        calibration_result = np.hstack((rotation_matrix, translation_vector.reshape(3, 1)))
        calibration_result = np.vstack((calibration_result, np.array([0, 0, 0, 1])))

        messagebox.showinfo("提示", "手眼标定完成，得到变换矩阵：\n{}".format(calibration_result))
    except Exception as e:
        messagebox.showerror("错误", "标定过程出现错误：{}".format(str(e)))


# 采集数据函数（用于图像标定）
def collect_data_image():
    """
    从图像文件中采集手眼数据，读取真实图像及关联对应的机器人手位姿信息
    """
    global current_step
    current_step += 1
    update_step_text()
    image_files = glob.glob("*.jpg") + glob.glob(r"c:\*.png")  # 获取当前目录下所有图像文件（可按实际指定路径）
    if not image_files:
        messagebox.showerror("错误", "未找到可用的图像文件，请确保当前目录下有包含棋盘格的图像文件（支持jpg、png格式）。")
        return
    for image_file in image_files:
        image = cv2.imread(image_file)
        gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
        ret, corners = cv2.findChessboardCorners(gray, board_size, None)
        if ret:
            # 假设对应的机器人手位姿信息存储在同名的.txt文件中（格式自定义，这里简单按行读取模拟解析）
            pose_file = os.path.splitext(image_file)[0] + ".txt"
            if os.path.exists(pose_file):
                with open(pose_file, 'r') as f:
                    pose_data = [float(line.strip()) for line in f.readlines()]
                    pose_matrix = np.array(pose_data).reshape(4, 4)
                    hand_poses.append(pose_matrix)
                    image_points.append(corners)
                    object_points.append(objp[0])
                    messagebox.showinfo("提示", f"已成功采集一组手眼数据（从 {image_file}）！")
            else:
                messagebox.showerror("错误", f"未找到对应的机器人手位姿文件 {pose_file}，请检查文件是否存在及命名规范。")
        else:
            messagebox.showerror("错误", f"在图像 {image_file} 中未检测到棋盘格，请检查图像内容。")

    if not hand_poses or not image_points:
        messagebox.showerror("错误", "采集的数据不完整，无法进行标定，请检查图像及对应位姿文件情况。")


# 执行图像标定函数
def perform_calibration_image():
    """
    执行基于图像文件的手眼标定操作，利用采集的数据计算手眼之间的变换关系
    """
    global current_step, calibration_result
    current_step += 1
    update_step_text()
    # 提醒检查数据数量是否足够
    messagebox.showinfo("操作提示", "首先会检查采集的数据数量是否足够用于标定，需要至少采集4组数据，如果不足会弹出错误提示框并终止标定操作，请确保之前已采集足够的数据。")
    if len(hand_poses) < 4 or len(image_points) < 4:
        messagebox.showerror("错误", "采集的数据不足，请至少采集4组数据进行标定！")
        return
    try:
        # 使用OpenCV的手眼标定函数（这里以Tsai-Lenz算法为例，实际可能有多种选择）
        # 提醒关于标定算法及参数传入相关操作
        messagebox.showinfo("操作提示", "接下来将调用cv2.calibrateHandEye函数进行手眼标定，采用的是Tsai-Lenz算法，该函数需要传入机器人手的旋转矩阵列表（从hand_poses中提取每个位姿的前3x3部分表示旋转）、平移向量列表（提取每个位姿的前3x3列第4行表示平移）、图像中棋盘格角点坐标列表（转换格式并转换数据类型为合适的numpy数组形式）以及真实世界中的棋盘格角点坐标列表（object_points）作为参数。")
        retval, rotation_vector, translation_vector = cv2.calibrateHandEye(
            [pose[:3, :3] for pose in hand_poses],
            [pose[:3, 3] for pose in hand_poses],
            [points.reshape(-1, 2).astype(np.float64) for points in image_points],
            object_points
        )

        # 将旋转向量和平移向量转换为变换矩阵形式方便后续使用
        rotation_matrix, _ = cv2.Rodrigues(rotation_vector)
        calibration_result = np.hstack((rotation_matrix, translation_vector.reshape(3, 1)))
        calibration_result = np.vstack((calibration_result, np.array([0, 0, 0, 1])))

        messagebox.showinfo("提示", "手眼标定完成（基于图像文件），得到变换矩阵：\n{}".format(calibration_result))
    except Exception as e:
        messagebox.showerror("错误", "标定过程出现错误：{}".format(str(e)))


# 显示采集的数据函数（可用于查看已采集的数据情况，方便调试等）
def show_collected_data():
    """
    在UI中以文本形式简单展示已采集的数据组数、手的位姿示例、图像点坐标示例等信息
    """
    data_info = "已采集数据组数：{}\n\n".format(len(hand_poses))
    if hand_poses:
        data_info += "手的位姿示例（第一个）：\n{}\n\n".format(hand_poses[0])
    if image_points:
        data_info += "图像点坐标示例（第一个）：\n{}\n\n".format(image_points[0])
    messagebox.showinfo("已采集数据详情", data_info)


# 重置操作函数，用于重新开始整个手眼标定流程
def reset_operation():
    """重置所有相关变量，重新开始手眼标定流程"""
    global hand_poses, image_points, object_points, calibration_result, current_step
    hand_poses = []
    image_points = []
    object_points = []
    calibration_result = None
    current_step = 0
    update_step_text()
    messagebox.showinfo("提示", "已重置操作，可重新开始手眼标定流程。")


root = tk.Tk()
root.title("手眼标定操作指引")
step_text = tk.StringVar()

# 在UI上显示步骤文本的标签
step_label = Label(root, textvariable=step_text)
step_label.pack(pady=10)

# 实时标定选择按钮
real_time_btn = Button(root, text="实时标定", command=lambda: (reset_operation(), set_calibration_mode('real_time')))
real_time_btn.pack(side=tk.LEFT, padx=10, pady=10)

# 图像标定选择按钮
image_btn = Button(root, text="图像标定", command=lambda: (reset_operation(), set_calibration_mode('image')))
image_btn.pack(side=tk.RIGHT, padx=10, pady=10)

# 用于记录当前选择的标定模式的变量
calibration_mode = None


def set_calibration_mode(mode):
    """设置当前标定模式，并更新相关按钮的命令绑定"""
    global calibration_mode
    calibration_mode = mode
    if mode == 'real_time':
        collect_btn.config(command=collect_data_real_time)
        calibrate_btn.config(command=perform_calibration_real_time)
    elif mode == 'image':
        collect_btn.config(command=collect_data_image)
        calibrate_btn.config(command=perform_calibration_image)


# 采集数据按钮（初始时无具体绑定，后续根据选择的标定模式绑定相应函数）
collect_btn = Button(root, text="采集手眼数据", command=None)
collect_btn.pack(pady=10)

# 执行标定按钮（初始时无具体绑定，后续根据选择的标定模式绑定相应函数）
calibrate_btn = Button(root, text="执行手眼标定", command=None)
calibrate_btn.pack(pady=10)

# 显示采集数据按钮
show_data_btn = Button(root, text="显示采集的数据", command=show_collected_data)
show_data_btn.pack(pady=10)

# 重置操作按钮
reset_btn = Button(root, text="重置操作", command=reset_operation)
reset_btn.pack(pady=10)

update_step_text()
root.mainloop()