工業機器人手眼標定-九點標定法的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