2D相机12点标定RAPID完整实现

1. 在使用2D相机进行引导时，如果已知机器人的TCP或者标定的针尖相对机器人法兰盘没有偏移，则可以使用9点标定（视觉9点标定原理及RAPID计算）。若TCP位置且机器人上特征相对法兰盘有偏心，则需要使用12点标定（9点平移+3点旋转）进行标定。

2. 此类功能在Hik等视觉软件中已经集成，也可以在机器人侧实现标定计算（相机仅给出特征的像素坐标）

3. 假设相机固定安装底拍（顶拍相同），机器人末端有一特征。机器人按照文首的轨迹（使用tool0）进行平移和旋转（前9个点平移，第10个点为移动到中间点5后绕法兰盘tool0的位置旋转-θ°，第11个点与第5点相同，第12个点为移动到中间点5后绕法兰盘tool0的位置旋转θ°）

PERS num cam_x:=0.00189328;PERS num cam_y:=3E-9;PERS num count:=1;PERS point testPoint:=[705.005,-7.10543E-15];
PERS num calib_x:=10; ! 标定移动时的x距离PERS num calib_y:=10; ! 标定移动时的y距离PERS num calib_rz:=5; ! 标定移动时的绕tool0旋转的角度
PROC main()    MoveJ p0,v1000,fine,tool0\WObj:=wobj0;    ! p0为5号点    count:=1;    rCalib p0,calib_x,calib_y,calib_rz;    stop;    rCam2;    calNewPoint [cam_x,cam_y],testPoint;    ! 拍照，可以测试像素cam_x，cam_y 转化到机器人base下的坐标是否正确     ENDPROC
PROC rCalib(robtarget p,num x,num y,num angle)    MoveL offs(p,-x,-y,0),v100,fine,tool0;    rCam;    MoveL offs(p,0,-y,0),v100,fine,tool0;    rCam;    MoveL offs(p,x,-y,0),v100,fine,tool0;    rCam;    MoveL offs(p,x,0,0),v100,fine,tool0;    rCam;    MoveL offs(p,0,0,0),v100,fine,tool0;    rCam;    MoveL offs(p,-x,0,0),v100,fine,tool0;    rCam;    MoveL offs(p,-x,y,0),v100,fine,tool0;    rCam;    MoveL offs(p,0,y,0),v100,fine,tool0;    rCam;    MoveL offs(p,x,y,0),v100,fine,tool0;    rCam;    MoveL offs(p,0,0,0),v100,fine,tool0;    !!! rotate    MoveL rotWobj(p,0,0,-angle),v100,fine,tool0;    rCam;    MoveL rotWobj(p,0,0,0),v100,fine,tool0;    rCam;    MoveL rotWobj(p,0,0,angle),v100,fine,tool0;    rCam;    calibCal;ENDProc
PROC calibCal()    VAR pos posCir{3};    VAR point realPoint;    VAR pos posCenter;    VAR num r;    VAR pos n;        calculateAffineMatrix\PointNum:=9,img_points,real_points,Affine;    ! 前9点完成平移的标定        ! 将10，11 ，12点的像素坐标转到前面变化后的结果    calNewPoint img_points{10},realPoint;    posCir{1}.x:=realPoint.x;    posCir{1}.y:=realPoint.y;
    calNewPoint img_points{11},realPoint;    posCir{2}.x:=realPoint.x;    posCir{2}.y:=realPoint.y;
    calNewPoint img_points{12},realPoint;    posCir{3}.x:=realPoint.x;    posCir{3}.y:=realPoint.y;
    fitcircle posCir,posCenter,r,n;    Affine{3}:=Affine{3}+(posCir{2}.x-posCenter.x);    Affine{6}:=Affine{6}+(posCir{2}.y-posCenter.y);    ! 补偿旋转中心ENDPROC
FUNCrobtarget rotWobj(robtarget p,num rx,num ry,num rz)    VAR orient o;    o:=p.rot;    o:=orientzyx(rz,ry,rx)*o;    p.rot:=o;    RETURN p;ENDFUNC
PROC rCam2()    waittime 0.5;    PulseDO do_cam;    waittime 0.5;    ! 触发拍照ENDPROC
PROC rCam()    VAR robtarget p;    rCam2;    p:=CRobT();    img_points{count}.x:=cam_x;    img_points{count}.y:=cam_y;    real_points{count}.x:=p.trans.x;    real_points{count}.y:=p.trans.y;    incr count;ENDPROC

MODULE affTrans    RECORD point        num x;        num y;    ENDRECORD    PERS point img_points{12}:=[[-0.00667989,-0.01], [0.00332009,-0.01], [0.0133201,-0.01], [0.0133201,0], [0.00332009,0], [-0.0066799,0],                                 [-0.00667989,0.01], [0.00332009,0.01], [0.0133201,0.01],[0.00262382,-0.0159475],                               [0.00332009,0], [0.00262382,0.0159475]];    PERS point real_points{12}:=[[512.014,-10],  [522.014,-10],[532.014,-10], [532.014,-2.15548E-14],[522.014,-2.11713E-14],[532.014,10],                                [522.014,-7.80626E-15],                               [522.014,-1.86001E-14],[522.014,-3.98986E-14]];
    PERS num Affine{6}:=[999.998,-1.16251E-13,701.685,5.28272E-15,1000,-1.42284E-14];    ! 仿射变换结果（包括旋转中心的补偿）    PERS point NewPointReal:=[683.18,0];    PERS point NewPointImg:=[250,250];
    PROC calNewPoint(point img_points,INOUT point real_points)        real_points.x:=Affine{1}*img_points.x+Affine{2}*img_points.y+Affine{3};        real_points.y:=Affine{4}*img_points.x+Affine{5}*img_points.y+Affine{6};     ENDPROC
    PROC calculateAffineMatrix(\num PointNum,point img_points{*},point real_points{*},INOUT num affine_matrix{*}\INOUT pos affine_pos)        VAR dnum A{20,6};        VAR dnum b{20};        VAR dnum x{6};        VAR num A_m;
        A_m:=Dim(img_points,1)*2;        IF Present(PointNum) THEN            A_m:=PointNum*2;        ENDIF        ! Ax = b          FOR i FROM 1 TO A_m/2 DO            A{2*i-1,1}:=NumToDnum(img_points{i}.x);            A{2*i-1,2}:=NumToDnum(img_points{i}.y);            A{2*i-1,3}:=1;            A{2*i-1,4}:=0;            A{2*i-1,5}:=0;            A{2*i-1,6}:=0;
            A{2*i,1}:=0;            A{2*i,2}:=0;            A{2*i,3}:=0;            A{2*i,4}:=NumToDnum(img_points{i}.x);            A{2*i,5}:=NumToDnum(img_points{i}.y);            A{2*i,6}:=1;
            b{2*i-1}:=NumToDnum(real_points{i}.x);            b{2*i}:=NumToDnum(real_points{i}.y);        ENDFOR
        !  x =[a, b, tx, c, d, ty ]        MatrixSolve A\A_m:=A_m,b,x;        FOR i FROM 1 TO 6 DO            affine_matrix{i}:=DnumToNum(x{i});        ENDFOR    ENDPROCENDMODULE

********************************