writen by wqj1212@yahoo.com.cn
位置0到位置1 各个参数关系位置0 末端到机器人基坐标位置----PulseNO.S 82501,L 69953,U -114458,R -2,B -1,T 1----CartPos.S 190.184,L 355.823,U -141.89,R 180,B -4.86,T 61.88a=180/180 *pi;b=-4.86/180 *pi;r=61.88/180*pi;Rx=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];Ry=[cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];Rz=[cos(r) -sin(r) 0;sin(r) cos(r) 0;0 0 1];R=Rz*Ry*RxR = 0.4696 0.8820 0.0399 0.8788 -0.4713 0.0747 0.0847 0.0000 -0.9964G = [0.4696 0.8820 0.0399 190.184; 0.8788 -0.4713 0.0747 355.823; 0.0847 0.0000 -0.9964 -141.89; 0 0 0 1 ];0 位置图像外参数Extrinsic parameters:Translation vector: Tc_ext = [ -15.564192 -31.592337 447.897520 ]Rotation vector: omc_ext = [ -2.359346 -2.071994 0.076281 ]Rotation matrix: Rc_ext = [ 0.128482 0.991024 -0.036917 0.991056 -0.129661 -0.031550 -0.036054 -0.032533 -0.998820 ]Pixel error: err = [ 0.15477 0.26929 ]G = [ 0.128482 0.991024 -0.036917 -15.564192 ; 0.991056 -0.129661 -0.031550 -31.592337 ; -0.036054 -0.032533 -0.998820 447.897520 ; 0 0 0 1 ];位置1 末端到机器人基坐标位置----PulseNO.S 75359,L 48202,U -107236,R -3,B -1,T 1----CartPos.S 214.629,L 324.508,U -101.991,R 180,B -10.24,T 56.52a=180/180 *pi;b=-10.24/180 *pi;r=56.52/180*pi;Rx=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];Ry=[cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];Rz=[cos(r) -sin(r) 0;sin(r) cos(r) 0;0 0 1];R=Rz*Ry*RxR = 0.5429 0.8341 0.0981 0.8208 -0.5516 0.1483 0.1778 0.0000 -0.9841G =[0.5429 0.8341 0.0981 214.629; 0.8208 -0.5516 0.1483 324.508; 0.1778 0.0000 -0.9841 -101.991; 0 0 0 1 ];1位置图像外参数Extrinsic parameters:Translation vector: Tc_ext = [ -63.060175 -21.839437 500.823439 ]Rotation vector: omc_ext = [ 2.391549 1.911963 -0.154389 ]Rotation matrix: Rc_ext = [ 0.218178 0.975410 -0.031214 0.967780 -0.220369 -0.121813 -0.125696 -0.003632 -0.992062 ]Pixel error: err = [ 0.11963 0.22145 ]G = [ 0.218178 0.975410 -0.031214 -63.060175; 0.967780 -0.220369 -0.121813 -21.839437 ; -0.125696 -0.003632 -0.992062 500.823439 ; 0 0 0 1 ];由位置0 到位置1摄像机外参数Bc1 1右图像外参数 Ac1 0右图像外参数Bc1 = [ 0.218178 0.975410 -0.031214 -63.060175; 0.967780 -0.220369 -0.121813 -21.839437 ; -0.125696 -0.003632 -0.992062 500.823439 ; 0 0 0 1 ];Ac1= [ 0.128482 0.991024 -0.036917 -15.564192 ; 0.991056 -0.129661 -0.031550 -31.592337 ; -0.036054 -0.032533 -0.998820 447.897520 ; 0 0 0 1 ];C1=Ac1*inv(Bc1)工具在基坐标系下 Ad1 0图像时，工具的姿态；Bd1 1图像时工具的姿态Ad1=[0.4696 0.8820 0.0399 190.184; 0.8788 -0.4713 0.0747 355.823; 0.0847 0.0000 -0.9964 -141.89; 0 0 0 1 ]; Bd1=[0.5429 0.8341 0.0981 214.629; 0.8208 -0.5516 0.1483 324.508; 0.1778 0.0000 -0.9841 -101.991; 0 0 0 1 ]; d1=inv(Ad1)*Bd1结果：右摄像机在A（0个右图像 ） B（1个右图像）两位置的关系C1 = [ 0.9958 -0.0896 0.0169 36.8261; 0.0907 0.9915 -0.0928 42.2616; -0.0084 0.0939 0.9955 -49.1724; 0 0 0 1.0000]; 工具在两位置的关系 d1 =[0.9913 -0.0931 0.0931 -12.6618; 0.0920 0.9956 0.0166 36.3170; -0.0942 -0.0079 0.9956 -41.1195; 0 0 0 1.0000];位置2 末端到机器人基坐标位置----PulseNO.S 86502,L 25258,U -100704,R -3,B -1,T 1----CartPos.S 153.909,L 328.212,U -70.151,R 180,B -15.1,T 64.87---Status: 1-cyclea=180/180 *pi;b=-15.1/180 *pi;r=64.87/180*pi;Rx=[1 0 0;0 cos(a) -sin(a);0 sin(a) cos(a)];Ry=[cos(b) 0 sin(b);0 1 0;-sin(b) 0 cos(b)];Rz=[cos(r) -sin(r) 0;sin(r) cos(r) 0;0 0 1];R=Rz*Ry*RxR = 0.4100 0.9053 0.1106 0.8741 -0.4247 0.2358 0.2605 0.0000 -0.9655G = [0.4100 0.9053 0.1106 153.909; 0.8741 -0.4247 0.2358 328.212; 0.2605 0.0000 -0.9655 -70.151; 0 0 0 1 ];位置2图像外参数Extrinsic parameters:Translation vector: Tc_ext = [ 14.913514 -27.935731 544.266948 ]Rotation vector: omc_ext = [ 2.201095 2.042448 -0.254298 ]Rotation matrix: Rc_ext = [ 0.070836 0.996832 -0.036178 0.975269 -0.076825 -0.207238 -0.209361 -0.020603 -0.977621 ]Pixel error: err = [ 0.08458 0.19457 ]G = [ 0.070836 0.996832 -0.036178 14.913514 ; 0.975269 -0.076825 -0.207238 -27.935731 ; -0.209361 -0.020603 -0.977621 544.266948 ; 0 0 0 1 ];由位置1-位置2摄像机外参数Bc2 2右图像外参数 Ac2 1右图像外参数Bc2 =[ 0.070836 0.996832 -0.036178 14.913514 ; 0.975269 -0.076825 -0.207238 -27.935731 ; -0.209361 -0.020603 -0.977621 544.266948 ; 0 0 0 1 ];Ac2=[ 0.218178 0.975410 -0.031214 -63.060175; 0.967780 -0.220369 -0.121813 -21.839437 ; -0.125696 -0.003632 -0.992062 500.823439 ; 0 0 0 1 ];C2=Ac2*inv(Bc2)工具在基坐标系下 Ad2 1图像时，工具的姿态；Bd2 2图像时工具的姿态Ad2=[0.5429 0.8341 0.0981 214.629; 0.8208 -0.5516 0.1483 324.508; 0.1778 0.0000 -0.9841 -101.991; 0 0 0 1 ]; Bd2= [0.4100 0.9053 0.1106 153.909; 0.8741 -0.4247 0.2358 328.212; 0.2605 0.0000 -0.9655 -70.151; 0 0 0 1 ]; d2=inv(Ad2)*Bd2结果：右摄像机在A（0个右图像 ） B（1个右图像）两位置的关系C2 = [0.9889 0.1443 -0.0353 -54.5862; -0.1467 0.9860 -0.0790 50.8843; 0.0234 0.0833 0.9963 -39.4254; 0 0 0 1.0000]; 工具在两位置的关系 d2 = [0.9863 0.1428 0.0819 -24.2573; -0.1403 0.9894 -0.0378 -52.6877; -0.0865 0.0258 0.9959 -36.7371; 0 0 0 1.0000];标定结果Rc1= [ 0.9958 -0.0896 0.0169; 0.0907 0.9915 -0.0928 ; -0.0084 0.0939 0.9955 ]; Rd1= [0.9913 -0.0931 0.0931 0.0920 0.9956 0.0166 -0.0942 -0.0079 0.9956]; Rc2=[0.9889 0.1443 -0.0353 -0.1467 0.9860 -0.0790 0.0234 0.0833 0.9963 ] Rd2= [0.9863 0.1428 0.0819 -0.1403 0.9894 -0.0378 -0.0865 0.0258 0.9959 ]; q=acos((trace(R)-1)/2);w(1,1)=q/(2*sin(q))*(R(3,2)-R(2,3));w(2,1)=q/(2*sin(q))*(R(1,3)-R(3,1));w(3,1)=q/(2*sin(q))*(R(2,1)-R(1,2))结果kc1 =[0.0936; 0.0127; 0.0904]; kc2=[ 0.0815; -0.0295; -0.1462]; kd1= [-0.0123; 0.0939; 0.0928]; kd2=[0.0320; 0.0846; -0.1422];kc3=cross(kc1,kc2);kd3=cross(kd1,kd2);a=[kc1 kc2 kc3]b=[kd1 kd2 kd3]R=a*inv(b)R=[kc1 kc2 cross(kc1,kc2)]*inv([kd1 kd2 cross(kd1,kd2)]) 最终 R = [ 0.0155 0.9835 0.0155; -0.9906 0.0131 -0.0077; -0.0147 -0.0328 1.0053];Rc1= [ 0.9958 -0.0896 0.0169; 0.0907 0.9915 -0.0928 ; -0.0084 0.0939 0.9955 ]; Rd1= [0.9913 -0.0931 0.0931 0.0920 0.9956 0.0166 -0.0942 -0.0079 0.9956]; Rc2=[0.9889 0.1443 -0.0353 -0.1467 0.9860 -0.0790 0.0234 0.0833 0.9963 ] Rd2= [0.9863 0.1428 0.0819 -0.1403 0.9894 -0.0378 -0.0865 0.0258 0.9959 ]; tc1= [36.8261;42.2616;-49.1724]; td1= [-12.6618;36.3170;-41.1195]; tc2= [-54.5862;50.8843;-39.4254]; td2= [-24.2573;-52.6877;-36.7371];c=R*td1-tc1;d=R*td2-tc2;a=Rc1-[1 0 0;0 1 0;0 0 1];b=Rc2-[1 0 0;0 1 0;0 0 1];h=[a;b];y=[c;d];t=inv(h'*h)*h'*y t = t = 7.7295 84.1694 312.1403