工业机器人手眼标定-九点标定法的C++实现

0,，九点标定法的具体实现方法参见，https://cloud.tencent.com/developer/article/1835302，本文只接受取到数据后的处理方法

1，我是将两个数据存放在两个txt文件内，CameraPos.txt存放是的相机坐标，RobotPos存在的是对应的机器人坐标

2，定义一个结构体存储标定后结果，定义两个vector<cv::Point2f>存储读取到的点坐标

public :
struct CalResult
    {
        double A_x;
        double B_x;
        double C_x;
        double A_y;
        double B_y;
        double C_y;
    }myCalResult;
public:
    vector<cv::Point2f> points_camera;
    vector<cv::Point2f> points_robot;

3，读取两个txt里的值，分别保存到两个vector

    char path[256];
    GetModuleFileNameA(NULL, path, 256);
    string filePath = path;
    filePath=filePath.substr(0, filePath.rfind('\\'));
    filePath = filePath + "\\"+ "CalData" + "\\" + "CameraPos.txt";

    ifstream cameraFile;
    cameraFile.open(filePath);
    assert(cameraFile.is_open());
    cv::Point2d temp;
    while (cameraFile.good() && !cameraFile.eof())
    {
        cameraFile >> temp.x >> temp.y;
        points_camera.push_back(temp);
    }
    filePath = filePath.substr(0, filePath.rfind('\\'));
    filePath = filePath + "\\" + "RobotPos.txt";
    ifstream robotFile;
    robotFile.open(filePath);
    assert(robotFile.is_open());
    while (robotFile.good() && !robotFile.eof())
    {
        robotFile >> temp.x >> temp.y;
        points_robot.push_back(temp);
    }

4，实现计算的函数

void getCalResult(vector<cv::Point2f> points_camera, vector<cv::Point2f> points_robot, CalResult a)
{
    if (points_camera.size()!= calPointCount ||  points_robot.size()!= calPointCount)
    {
        ::MessageBox(NULL,TEXT("手眼标定错误"),TEXT("错误"),1);
        return;
    }

    cv::Mat dst = cv::Mat(3, 3, CV_32F, cv::Scalar(0));//初始化系数矩阵A
    cv::Mat out_x = cv::Mat(3, 1, CV_32F, cv::Scalar(0));//初始化矩阵b
    cv::Mat out_y = cv::Mat(3, 1, CV_32F, cv::Scalar(0));//初始化矩阵b
    for (int i = 0; i < points_camera.size(); i++)
    {
        //计算3*3的系数矩阵
        dst.at<float>(0, 0) = dst.at<float>(0, 0) + pow(points_camera[i].x, 2);
        dst.at<float>(0, 1) = dst.at<float>(0, 1) + points_camera[i].x*points_camera[i].y;
        dst.at<float>(0, 2) = dst.at<float>(0, 2) + points_camera[i].x;
        dst.at<float>(1, 0) = dst.at<float>(1, 0) + points_camera[i].x*points_camera[i].y;
        dst.at<float>(1, 1) = dst.at<float>(1, 1) + pow(points_camera[i].y, 2);
        dst.at<float>(1, 2) = dst.at<float>(1, 2) + points_camera[i].y;
        dst.at<float>(2, 0) = dst.at<float>(2, 0) + points_camera[i].x;
        dst.at<float>(2, 1) = dst.at<float>(2, 1) + points_camera[i].y;
        dst.at<float>(2, 2) = points_camera.size();
        //x计算3*1的结果矩阵
        out_x.at<float>(0, 0) = out_x.at<float>(0, 0) + points_camera[i].x*points_robot[i].x;
        out_x.at<float>(1, 0) = out_x.at<float>(1, 0) + points_camera[i].y*points_robot[i].x;
        out_x.at<float>(2, 0) = out_x.at<float>(2, 0) + points_robot[i].x;

        //y计算3*1的结果矩阵
        out_y.at<float>(0, 0) = out_y.at<float>(0, 0) + points_camera[i].x*points_robot[i].y;
        out_y.at<float>(1, 0) = out_y.at<float>(1, 0) + points_camera[i].y*points_robot[i].y;
        out_y.at<float>(2, 0) = out_y.at<float>(2, 0) + points_robot[i].y;


    }
    //判断矩阵是否奇异
    double determ = determinant(dst);
    if (abs(determ) < 0.001) {
        ::MessageBox(NULL, TEXT("X标定求解奇异"), TEXT("错误"), 1);
        return;
    }
    cv::Mat inv;
    cv::invert(dst, inv);//求矩阵的逆
    cv::Mat output = inv * out_x;//计算输出

    //X坐标计算结果,robotX=A_x*Camera_X+B_x*Camera_Y+C_x
    a.A_x = output.at<float>(0, 0);
    a.B_x = output.at<float>(1, 0);
    a.C_x = output.at<float>(2, 0);

    output = inv * out_y;//计算输出
    //Y坐标计算结果,robotY=A_y*Camera_X+B_y*Camera_Y+C_y
    a.A_y = output.at<float>(0, 0);
    a.B_y = output.at<float>(1, 0);
    a.C_y = output.at<float>(2, 0);

}

6 计算结果验证 https://zhuanlan.zhihu.com/p/391938754

7 算法参考:https://blog.csdn.net/AlonewaitingNew/article/details/95217730