在视觉引导中 当相机视野与旋转中心很远的时候,使用旋转多个点来计算圆心准确性可能不高。今天分享使用视野内两点来计算旋转中心:
这里在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;
以上默认机械手与图像空间是同一个空间,实际在运用过程中需要标定到机械手空间,在结合以上的计算出机械手的旋转中心。