3D手眼标定系统详细说明文档

1. 系统概述

本系统是一个基于Windows Forms的3D手眼标定工具，主要用于机器人视觉应用中相机坐标系与机器人基座坐标系之间的精确标定。系统集成了相机标定、手眼标定、点云处理和抓取位姿计算等功能，为机器人视觉应用提供精确的坐标转换能力。

2. 系统架构

系统采用模块化设计，主要包含以下模块：

• 相机标定模块：使用棋盘格进行相机参数标定
• 手眼标定模块：计算相机相对于机器人基座的变换矩阵
• 点云处理模块：点云数据的加载、可视化和配准
• 用户界面模块：提供友好的图形用户界面

3. 主要功能模块说明

3.1 相机标定模块

该模块用于获取相机的内参（焦距、主点坐标等）和畸变系数。系统使用棋盘格进行标定，通过检测棋盘格角点来计算相机参数。

关键功能：

• 棋盘格图像导入
• 角点检测
• 相机参数计算
• 畸变校正

关键代码片段：

private void btn_calibrate_Click(object sender, EventArgs e)
{
    if (ChessBoardPath.Count > 0)
    {
        new Thread(new ThreadStart(() =>
        {
            try
            {
                string ErrorBack = string.Empty;
                if (CalibTool.Calibrate(ChessBoardPath, BoardNum, ErrorBack))
                {
                    toolStripStatusLabel1.Text = "标定成功！";
                    CalibSucceed = true;
                    DisplayParameter();
                    InitWrite();
                }
                else
                {
                    toolStripStatusLabel1.Text = "标定失败！" + ErrorBack;
                    bn_UnDistort.Visible = false;
                    pictureBox1.Image = Image.FromFile(ChessBoardPath[0]);
                    foreach (TextBox item in groupBox1.Controls)
                    {
                        item.Text = "";
                    }
                    foreach (TextBox item in groupBox2.Controls)
                    {
                        item.Text = "";
                    }
                }
            }
            catch (Exception Err)
            {
            }
            BeginInvoke(new System.Threading.ThreadStart(() =>
            {
                bn_UnDistort.Visible = true;
            }));
        })){IsBackground = true}.Start();
    }
    else
    {
        bn_UnDistort.Visible = false;
        toolStripStatusLabel1.Text = "无图像输入！";
    }
}

3.2 手眼标定模块

该模块用于计算相机相对于机器人基座的变换矩阵。系统需要输入机器人位姿（从DAT文件读取）和相机位姿（通过角点检测计算），然后使用这些数据计算变换矩阵。

关键功能：

• 机器人位姿数据读取
• 角点检测与相机位姿计算
• 手眼标定计算
• 标定结果评估

关键代码片段：

private void btnStartCalibration_Click(object sender, EventArgs e)
{
    // 获取参数
    dataDir = txtDataDir.Text;
    dataStart = txtDataStart.Text;
    numImages = (int)txtNumImages.Value;
    boardWidth = (int)txtBoardWidth.Value;
    boardHeight = (int)txtBoardHeight.Value;
    squareSize = (float)txtSquareSize.Value;
    
    CameraParameterArray[0] = CalibTool.CameraParameterArray[0, 0];
    CameraParameterArray[1] = 0;
    CameraParameterArray[2] = CalibTool.CameraParameterArray[1, 1];
    CameraParameterArray[3] = 0;
    CameraParameterArray[4] = CalibTool.CameraParameterArray[0, 2];
    CameraParameterArray[5] = CalibTool.CameraParameterArray[1, 2];
    CameraParameterArray[6] = 0;
    CameraParameterArray[7] = 0;
    CameraParameterArray[8] = 1;
    DistCoeffsArray[0] = CalibTool.DistCoeffsArray[0];
    DistCoeffsArray[1] = CalibTool.DistCoeffsArray[1];
    DistCoeffsArray[2] = CalibTool.DistCoeffsArray[2];
    DistCoeffsArray[3] = CalibTool.DistCoeffsArray[3];
    DistCoeffsArray[4] = CalibTool.DistCoeffsArray[4];
    
    // 验证参数
    if (string.IsNullOrEmpty(dataDir) || string.IsNullOrEmpty(dataStart))
    {
        MessageBox.Show("请设置数据目录路径和保存结果路径！", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
        return;
    }
    if (!Directory.Exists(dataDir))
    {
        MessageBox.Show("数据目录不存在！", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
        return;
    }
    if (!Directory.Exists(dataStart))
    {
        MessageBox.Show("保存结果路径不存在！", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
        return;
    }
    
    // 开始标定
    PerformCalibration();
}

3.3 点云处理模块

该模块用于加载、可视化和配准3D点云数据。系统支持PLY、OBJ、TXT和STL等格式的点云文件。

关键功能：

• 点云文件加载
• 点云可视化
• 点云配准（使用Fast Global Registration算法）
• 配准结果评估

关键代码片段：

private void button4_Click(object sender, EventArgs e)
{
    // 清空点云
    cloud1.Clear();
    
    // 1. 打开文件对话框：扩展支持的点云格式
    OpenFileDialog ofd = new OpenFileDialog();
    ofd.Title = "请选择点云文件";
    ofd.InitialDirectory = @"\";
    ofd.Filter = "支持的点云文件|*.ply;*.obj;*.txt;*.stl|PLY文件(*.ply)|*.ply|OBJ文件(*.obj)|*.obj|文本点云(*.txt)|*.txt|STL文件(*.stl)|*.stl|所有文件(*.*)|*.*";
    ofd.FilterIndex = 1; // 默认显示所有支持格式
    ofd.Multiselect = false; // 禁止多选
    
    // 若用户选择了文件
    if (ofd.ShowDialog() == DialogResult.OK)
    {
        url = ofd.FileName;
        string fileExtension = Path.GetExtension(url).ToLower(); // 获取文件扩展名（小写）
        sourceFilePath = url;
        
        try
        {
            // 2. 分格式加载点云
            switch (fileExtension)
            {
                case ".ply":
                    PCLSharp.Io.loadPlyFile(url, cloud1.PointCloudXYZPointer);
                    break;
                case ".obj":
                case ".txt":
                    PCLSharp.Io.loadTxtFile(url, cloud1.PointCloudXYZPointer);
                    break;
                case ".stl":
                    PCLSharp.Io.stl2PointCloud(url, cloud1.PointCloudXYZPointer);
                    break;
                default:
                    MessageBox.Show($"不支持的文件格式：{fileExtension}", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
                    break;
            }
            
            MessageBox.Show($"点云加载成功！点数：{cloud1.Size}", "成功", MessageBoxButtons.OK, MessageBoxIcon.Information);
        }
        catch (Exception ex)
        {
            MessageBox.Show($"加载失败：{ex.Message}", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
        }
    }
    
    vtkRenderer renderer = showPointCloud(cloud1);
    vtkRenderWindow renWin = renderWindowControl1.RenderWindow;
    renWin.AddRenderer(renderer);
    panel1.Refresh();
}

3.4 抓取位姿计算模块

该模块用于计算机器人抓取目标点的位姿。系统通过输入抓取点的6维位姿（位置和姿态），结合手眼标定结果，计算机器人基座坐标系中的位姿。

关键功能：

• 抓取点位姿输入
• 位姿转换计算
• 位姿结果输出

关键代码片段：

private void CalculateGrabPose()
{
    // 获取输入的抓取点（6维位姿：位置x, y, z 和姿态rx, ry, rz）
    if (!double.TryParse(txtGrabX.Text, out double x) || 
        !double.TryParse(txtGrabY.Text, out double y) || 
        !double.TryParse(txtGrabZ.Text, out double z) || 
        !double.TryParse(txtGrabRx.Text, out double rx) || 
        !double.TryParse(txtGrabRy.Text, out double ry) || 
        !double.TryParse(txtGrabRz.Text, out double rz))
    {
        MessageBox.Show("请正确输入抓取点的6维位姿（位置和姿态）！", "错误", MessageBoxButtons.OK, MessageBoxIcon.Error);
        return;
    }
    
    // 将欧拉角（度）转换为弧度
    double rxRad = rx * Math.PI / 180.0;
    double ryRad = ry * Math.PI / 180.0;
    double rzRad = rz * Math.PI / 180.0;
    
    // 从resultTransform获取4x4变换矩阵
    double[] transform = resultTransform;
    
    // 计算相机坐标系中的旋转矩阵（ZYX顺序）
    double cx = Math.Cos(rxRad);
    double sx = Math.Sin(rxRad);
    double cy = Math.Cos(ryRad);
    double sy = Math.Sin(ryRad);
    double cz = Math.Cos(rzRad);
    double sz = Math.Sin(rzRad);
    
    // 构建旋转矩阵 R_cam
    double[] R_cam = new double[9];
    R_cam[0] = cz * cy; // R[0,0]
    R_cam[1] = cz * sy * sx - sz * cx; // R[0,1]
    R_cam[2] = cz * sy * cx + sz * sx; // R[0,2]
    R_cam[3] = sz * cy; // R[1,0]
    R_cam[4] = sz * sy * sx + cz * cx; // R[1,1]
    R_cam[5] = sz * sy * cx - cz * sx; // R[1,2]
    R_cam[6] = -sy; // R[2,0]
    R_cam[7] = cy * sx; // R[2,1]
    R_cam[8] = cy * cx; // R[2,2]
    
    // 提取手眼标定变换矩阵的旋转部分（3x3）
    double[] R_transform = new double[9];
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            R_transform[i * 3 + j] = transform[i * 4 + j];
        }
    }
    
    // 提取手眼标定变换矩阵的平移向量（3x1）
    double[] t_transform = new double[3];
    t_transform[0] = transform[0 * 4 + 3]; // x
    t_transform[1] = transform[1 * 4 + 3]; // y
    t_transform[2] = transform[2 * 4 + 3]; // z
    
    // 计算最终的旋转矩阵 R_final = R_transform * R_cam
    double[] R_final = new double[9];
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            R_final[i * 3 + j] = 0;
            for (int k = 0; k < 3; k++)
            {
                R_final[i * 3 + j] += R_transform[i * 3 + k] * R_cam[k * 3 + j];
            }
        }
    }
    
    // 计算最终的位置向量 t_final = R_transform * t_cam + t_transform
    double[] t_cam = { x, y, z };
    double[] t_final = new double[3];
    for (int i = 0; i < 3; i++)
    {
        t_final[i] = t_transform[i];
        for (int k = 0; k < 3; k++)
        {
            t_final[i] += R_transform[i * 3 + k] * t_cam[k];
        }
    }
    
    // 将最终的旋转矩阵转换为欧拉角（ZYX顺序）
    double sy_final = Math.Sqrt(R_final[0] * R_final[0] + R_final[1] * R_final[1]);
    bool singular_final = sy_final < 1e-6;
    double rx_final, ry_final, rz_final;
    if (!singular_final)
    {
        rx_final = Math.Atan2(R_final[6], R_final[8]) * 180.0 / Math.PI;
        ry_final = Math.Atan2(-R_final[2], sy_final) * 180.0 / Math.PI;
        rz_final = Math.Atan2(R_final[1], R_final[0]) * 180.0 / Math.PI;
    }
    else
    {
        rx_final = Math.Atan2(-R_final[5], R_final[4]) * 180.0 / Math.PI;
        ry_final = Math.Atan2(-R_final[2], sy_final) * 180.0 / Math.PI;
        rz_final = 0;
    }
    
    // 添加到抓取位姿列表
    GrabPose pose = new GrabPose()
    {
        Index = grabPoses.Count + 1,
        X = t_final[0] * 1000, // 转换为mm
        Y = t_final[1] * 1000, // 转换为mm
        Z = t_final[2] * 1000, // 转换为mm
        Rx = rx_final,
        Ry = ry_final,
        Rz = rz_final
    };
    grabPoses.Add(pose);
    
    // 更新界面
    UpdateGrabPoses();
}

4. 关键算法说明

4.1 手眼标定算法

手眼标定是系统的核心，用于计算相机坐标系与机器人基座坐标系之间的变换关系。系统采用基于机器人位姿和相机位姿的标定方法。

算法流程：

1. 获取相机内参和畸变系数
2. 读取机器人位姿数据（从DAT文件）
3. 检测图像中的棋盘格角点，计算相机位姿
4. 使用标定算法计算相机到基座的变换矩阵
5. 评估标定结果的精度

关键代码：

int result = completeHandEyeCalibrationWithPoses(dataDir, outputDir, numImages, boardWidth, boardHeight, squareSize, tempResultTransform, 
    out calibrationError, robotPosesArray, cameraPosesArray, out validPoseCount, CameraParameterArray, DistCoeffsArray);

4.2 点云配准算法

系统使用Fast Global Registration (FGR)算法进行点云配准，该算法是一种高效的点云配准方法，能够快速找到点云之间的最优变换。

算法特点：

• 高效的特征描述
• 快速的初始匹配
• 优化的变换计算

关键代码：

double[] transformation;
double fitness, inlierRmse;
infos = FastGlobalRegistration_Master_P.FastGlobalRegistration_ByData_Demo(sourceXyz, sourceCount, targetXyz, targetCount, savePath, out transformation, out fitness, out inlierRmse, sourceFilePath, rcp001, rcp002, rcp003, rcp004, useAbsoluteScale: false, tupleTest: true);

5. 使用说明

5.1 系统启动

1. 启动应用程序
2. 系统自动加载默认设置

5.2 相机标定流程

1. 准备棋盘格图像（至少10张）
2. 点击"浏览图像"按钮选择图像目录
3. 点击"标定"按钮开始标定
4. 标定成功后，系统显示相机参数

5.3 手眼标定流程

1. 准备数据：
   • 图像数据：PNG格式的图像
   • 机器人位姿数据：DAT格式的文件（每张图像对应一个DAT文件）
2. 设置参数：
   • 图像数量
   • 棋盘格尺寸
   • 棋盘格方格大小
3. 点击"开始标定"按钮
4. 系统计算相机到基座的变换矩阵
5. 标定成功后，系统显示结果并保存

5.4 点云配准流程

1. 点击"加载源点云"按钮加载源点云
2. 点击"加载目标点云"按钮加载目标点云
3. 点击"配准"按钮进行点云配准
4. 系统显示配准结果和配准参数

5.5 抓取位姿计算流程

1. 先完成手眼标定
2. 在输入框中输入抓取点的6维位姿（X, Y, Z, Rx, Ry, Rz）
3. 点击"计算抓取位姿"按钮
4. 系统计算机器人基座坐标系中的抓取位姿并显示

6. 系统特点

1. 模块化设计：各功能模块独立，便于维护和扩展
2. 多格式支持：支持多种图像和点云格式
3. 高精度计算：使用先进的标定和配准算法
4. 用户友好界面：直观的图形界面，操作简单
5. 实时反馈：标定过程中提供实时进度反馈

7. 技术栈

• 编程语言：C#
• 图形库：OpenCvSharp, VTK
• 点云处理：PCLSharp
• UI框架：Windows Forms

8. 系统应用

该系统广泛应用于机器人视觉领域，如：

• 机器人抓取和装配
• 3D测量和检测
• 机器人导航和定位
• 自动化生产线

通过精确的手眼标定，系统能够实现机器人对目标物体的精确定位和抓取，提高自动化系统的精度和效率。

---

注：本系统为3D手眼标定工具，需配合实际硬件设备使用。标定结果的精度取决于相机标定的精度和手眼标定过程中数据的准确性。

需要源码加好友交流沟通