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
    ENDPROC
ENDMODULE

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