三点找圆

机器人手眼标定功能的一部分

计划采用九点标定，九点标定通过变换矩阵就可以完成，在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;
}