機器人手眼標定功能的一部分

計劃採用九點標定，九點標定通過變換矩陣就可以完成，在opencv中就是一個api，但是問題是無法很好的進行機器人座標與影象點的對應

解釋一下問題：

機器人自己有個座標系，機器人照片採集的影象也有一個座標系，手眼標定就是將影象的座標系轉換成機器人的座標系，相當於告訴機器人，你拍到的影象中的這個座標點就相當於你自己座標系中的那個座標點，多找幾個這樣的點就可以求出變換矩陣。所以在開始找點的時候，我要讓機器手或者探針指向機器人相機採集到的影象上的一個點，影象上的這個點座標就對應機器人座標系的座標。問題是我可以通過指令控制機器人，我知道這個點在機器人座標系中的位置，但是我不知道這個點在影象中的精確位置，我只能粗略的告訴機器人，這個點大概在什麼位置，所以為了解決這個問題，就使用旋轉找圓策略。

旋轉找圓策略就是讓機器手或者探針固定一個標定板進行旋轉，取三幅旋轉影象，通過角點檢測獲得特定的點，opencv有精確到畫素的高精度檢測演算法，將三幅旋轉影象的點進行找圓，圓心就是機器手在影象中的精確座標，這樣就可以進行對應。

找圓部分程式碼

#include"pch.h"
#include<iostream>
#include<opencv2/opencv.hpp>
#include<math.h>
using namespace std;
using namespace cv;

class CIRCLEDATA
{
public:
    int bugnum;
    Point2f center_point;
    
 
float radius;

public:
    CIRCLEDATA()
    {
        bugnum = 0;
    }
};

CIRCLEDATA getCircle(Point2f p1, Point2f p2, Point2f p3)
{
    CIRCLEDATA mycircle;
    Point2f tmp;

    //三點一線
    if ((p1.y == p2.y) && (p2.y == p3.y))
    {
        mycircle.bugnum = 1;
        return mycircle;
    }

    
 
//三點一線
    if ((p1.x == p2.x) && (p2.x == p3.x))
    {
        mycircle.bugnum = 1;
        return mycircle;
    }

    //有重複點
    if (((p1.x == p2.x) && (p1.y == p2.y))||((p2.x==p3.x)&&(p2.y==p3.y))||((p1.x==p3.x)&&(p1.y==p3.y)))
    {
        mycircle.bugnum = 2;
        return mycircle;
    }

    //輸入三點中前兩點y值相同導致計算k1分母為0
    //將p2和p3交換，這樣計算k1k2的分母都不會為0
    if (p1.y == p2.y)
    {
        tmp.x = p2.x;
        tmp.y = p2.y;
        p2.x = p3.x;
        p2.y = p3.y;
        p3.x = tmp.x;
        p3.y = tmp.y;
    }

    //輸入三點中後兩點y值相同導致計算k2分母為0
    //同理交換p1p2
    if (p2.y == p3.y)
    {
        tmp.x = p1.x;
        tmp.y = p1.y;
        p1.x = p2.x;
        p1.y = p2.y;
        p2.x = tmp.x;
        p2.y = tmp.y;
    }

    //計算兩兩連線的中點
    Point2f mid1, mid2;
    mid1.x = (p1.x + p2.x) / 2;
    mid1.y = (p1.y + p2.y) / 2;

    mid2.x = (p3.x + p2.x) / 2;
    mid2.y = (p2.y + p3.y) / 2;

    //計算兩個連線的斜率
    float k1 = -(p1.x - p2.x) / (p1.y - p2.y);
    float k2 = -(p2.x - p3.x) / (p2.y - p3.y);

    //三點一線
    if (k1 == k2)
    {
        mycircle.bugnum = 1;
        return mycircle;
    }
    //計算圓的中心點
    mycircle.center_point.x = (k1*mid1.x - k2 * mid2.x + mid2.y - mid1.y) / (k1 - k2);
    mycircle.center_point.y = (k1*mid2.y - k2 * mid1.y + (k1*k2)*(mid1.x - mid2.x)) / (k1 - k2);
    
    //計算圓的半徑
    mycircle.radius = sqrtf((mycircle.center_point.x - p1.x)*(mycircle.center_point.x - p1.x) + (mycircle.center_point.y - p1.y)*(mycircle.center_point.y - p1.y));
    return mycircle;
}
int main(int argc, char**argv)
{
    //輸入點資訊
    Point2f p1, p2, p3;
    cout << "請輸入第一個點的橫縱座標" << endl;
    cin >> p1.x;
    cin >> p1.y;

    cout << "請輸入第二個點的橫縱座標" << endl;
    cin >> p2.x;
    cin >> p2.y;

    cout << "請輸入第三個點的橫縱座標" << endl;
    cin >> p3.x;
    cin >> p3.y;

    //錯誤檢測
    CIRCLEDATA c=getCircle(p1, p2, p3);
    if (1 == c.bugnum)
    {
        cout << "三點成一條直線，無法成圓" << endl;
        return 0;
    }

    if (2 == c.bugnum)
    {
        cout << "輸入點有重複" << endl;
        return 0;
    }

    //生成背景圖
    Mat bg(800, 800, CV_8UC3, Scalar(0, 0, 0));

    //畫點
    circle(bg, p1, 2, Scalar(0, 0, 255), 1, 8, 0);
    circle(bg, p2, 2, Scalar(0, 0, 255), 1, 8, 0);
    circle(bg, p3, 2, Scalar(0, 0, 255), 1, 8, 0);

    //畫圓心和半徑
    circle(bg, c.center_point, 1, Scalar(0, 255, 0), 1, 8, 0);
    circle(bg, c.center_point, c.radius, Scalar(0, 255, 0), 1, 8, 0);
    imshow("show", bg);

    //輸出生成圓的資訊
    cout << "bug型別"<<c.bugnum << endl;
    cout << "圓心橫座標"<<c.center_point.x << endl;
    cout<<"圓心縱座標"<<c.center_point.y << endl;
    cout <<"圓半徑"<< c.radius << endl;
    waitKey(0);
    //system("pause");
    return 0;
}