两点计算机械手旋转中心

在视觉引导中 当相机视野与旋转中心很远的时候，使用旋转多个点来计算圆心准确性可能不高。今天分享使用视野内两点来计算旋转中心：

这里在Visionpro里用图像做了验证

P2是P1特征点绕O旋转角度A后的点 P3是P1和P2的延长线与以P1为圆心半径为r的焦点 直接上代码吧

 double d;//P1和P2直接的距离
    double x0,y0;//旋转中心O
    double x1,y1,x2,y2;//p1和P2坐标
    double a;//旋转角度
    double xx,yy;//P3坐标
    double r ;//旋转半径
   
    x1 = this.Tools.CogIntersectLineCircleTool1.X1;
    y1 = this.Tools.CogIntersectLineCircleTool1.Y1;
    x2 = this.Tools.CogIntersectLineCircleTool2.X1;
    y2 = this.Tools.CogIntersectLineCircleTool2.Y1;
    
    d = Math.Sqrt((x2 - x1) * (x2 - x1)+(y2 - y1) * (y2 - y1));
    a =  this.Tools.CogAngleLineLineTool1.Angle;//a为弧度
    
    r = d / 2 / Math.Sin(a / 2);//
    
    //r / d = (xx - x1) / (x2 - x1);
    //r / d = (yy - y1) / (y2 - y1);
    xx = (1 - r / d) * x1 + (r / d) * x2;
    yy = (1 - r / d) * y1 + (r / d) * y2;
    
    double rr = 90 - (CogMisc.RadToDeg(a)/ 2);
    
    x0 = Math.Cos(Math.PI/2 - a / 2) * (xx - x1) - Math.Sin(Math.PI/2 - a / 2) * (yy - y1) + x1;
    y0 = Math.Cos(Math.PI/2 - a / 2) * (yy - y1) + Math.Sin(Math.PI/2 - a / 2) * (xx - x1) + y1;

以上默认机械手与图像空间是同一个空间，实际在运用过程中需要标定到机械手空间，在结合以上的计算出机械手的旋转中心。