⭐ 欢迎来到虫虫下载站！ | 📦 资源下载 📁 资源专辑 ℹ️ 关于我们
⭐ 虫虫下载站
📤 上传资源

📄 all.m

📁 这是一个机器人视觉的摄像机标定的程序
💻 M
字号:
🔍 
%you摄像机标定用数据
format long;
u1=123;
v1=62;
Xw1=0;
Yw1=0;
Zw1=34;

u2=288;
v2=67;
Xw2=40;
Yw2=0;
Zw2=26;

u3=443
v3=73;
Xw3=80;
Yw3=0;
Zw3=18;

u4=606;
v4=73;
Xw4=120;
Yw4=0;
Zw4=26;

u5=124;
v5=224;
Xw5=0;
Yw5=40;
Zw5=34;

u6=286;
v6=228;
Xw6=40;
Yw6=40;
Zw6=30;

u7=445;
v7=231;
Xw7=80;
Yw7=40;
Zw7=22;

u8=593;
v8=233;
Xw8=120;
Yw8=40;
Zw8=10;

u9=121;
v9=390;
Xw9=0;
Yw9=80;
Zw9=26;

u10=287;
v10=391;
Xw10=40;
Yw10=80;
Zw10=30;

u11=443;
v11=388;
Xw11=80;
Yw11=80;
Zw11=22;

u12=591;
v12=387;
Xw12=120;
Yw12=80;
Zw12=10;

u13=123;
v13=541;
Xw13=0;
Yw13=120;
Zw13=14;

u14=281;
v14=541;
Xw14=40;
Yw14=120;
Zw14=14;

u15=443;
v15=546;
Xw15=80;
Yw15=120;
Zw15=26;


Xw16=80;
Yw16=120;
Zw16=26;

A=[Xw1, Yw1, Zw1, 1,  0,  0,   0,   0, -u1*Xw1, -u1*Yw1, -u1*Zw1;
   0,    0,   0,  0, Xw1, Yw1, Zw1, 1, -v1*Xw1, -v1*Yw1, -v1*Zw1;
   Xw2, Yw2, Zw2, 1,  0,  0,   0,   0, -u2*Xw2, -u2*Yw2, -u2*Zw2;
   0,    0,   0,  0, Xw2, Yw2, Zw2, 1, -v2*Xw2, -v2*Yw2, -v2*Zw2;
   Xw3, Yw3, Zw3, 1,  0,  0,   0,   0, -u3*Xw3, -u3*Yw3, -u3*Zw3;
   0,    0,   0,  0, Xw3, Yw3, Zw3, 1, -v3*Xw3, -v3*Yw3, -v3*Zw3;
   Xw4, Yw4, Zw4, 1,  0,  0,   0,   0, -u4*Xw4, -u4*Yw4, -u4*Zw4;
   0,    0,   0,  0, Xw4, Yw4, Zw4, 1, -v4*Xw4, -v4*Yw4, -v4*Zw4;
   Xw5, Yw5, Zw5, 1,  0,  0,   0,   0, -u5*Xw5, -u5*Yw5, -u5*Zw5;
   0,    0,   0,  0, Xw5, Yw5, Zw5, 1, -v5*Xw5, -v5*Yw5, -v5*Zw5;
   Xw6, Yw6, Zw6, 1,  0,  0,   0,   0, -u6*Xw6, -u6*Yw6, -u6*Zw6;
   0,    0,   0,  0, Xw6, Yw6, Zw6, 1, -v6*Xw6, -v6*Yw6, -v6*Zw6;
   Xw7, Yw7, Zw7, 1,  0,  0,   0,   0, -u7*Xw7, -u7*Yw7, -u7*Zw7;
   0,    0,   0,  0, Xw7, Yw7, Zw7, 1, -v7*Xw7, -v7*Yw7, -v7*Zw7;
   Xw8, Yw8, Zw8, 1,  0,  0,   0,   0, -u8*Xw8, -u8*Yw8, -u8*Zw8;
   0,    0,   0,  0, Xw8, Yw8, Zw8, 1, -v8*Xw8, -v8*Yw8, -v8*Zw8;
   Xw9, Yw9, Zw9, 1,  0,  0,   0,   0, -u9*Xw9, -u9*Yw9, -u9*Zw9;
   0,    0,   0,  0, Xw9, Yw9, Zw9, 1, -v9*Xw9, -v9*Yw9, -v9*Zw9;
   Xw10, Yw10, Zw10, 1,  0,  0,   0,   0, -u10*Xw10, -u10*Yw10, -u10*Zw10;
   0,    0,   0,  0, Xw10, Yw10, Zw10, 1, -v10*Xw10, -v10*Yw10, -v10*Zw10;
   
   Xw11, Yw11, Zw11, 1,  0,  0,   0,   0, -u11*Xw11, -u11*Yw11, -u11*Zw11;
   0,    0,   0,  0, Xw11, Yw11, Zw11, 1, -v11*Xw11, -v11*Yw11, -v11*Zw11;
   Xw12, Yw12, Zw12, 1,  0,  0,   0,   0, -u12*Xw12, -u12*Yw12, -u12*Zw12;
   0,    0,   0,  0, Xw12, Yw12, Zw12, 1, -v12*Xw12, -v12*Yw12, -v12*Zw12;
   Xw13, Yw13, Zw13, 1,  0,  0,   0,   0, -u13*Xw13, -u13*Yw13, -u13*Zw13;
   0,    0,   0,  0, Xw13, Yw13, Zw13, 1, -v13*Xw13, -v13*Yw13, -v13*Zw13;
   Xw14, Yw14, Zw14, 1,  0,  0,   0,   0, -u14*Xw14, -u14*Yw14, -u14*Zw14;
   0,    0,   0,  0, Xw14, Yw14, Zw14, 1, -v14*Xw14, -v14*Yw14, -v14*Zw14;
   Xw15, Yw15, Zw15, 1,  0,  0,   0,   0, -u15*Xw15, -u15*Yw15, -u15*Zw15;
   0,    0,   0,  0, Xw15, Yw15, Zw15, 1, -v15*Xw15, -v15*Yw15, -v15*Zw15; 
    ];
  B=[u1,v1,u2,v2,u3,v3,u4,v4,u5,v5,u6,v6,u7,v7,u8,v8,u9,v9, u10,v10,u11,v11,u12,v12,u13,v13,u14,v14,u15,v15]
  B=B';
  % 运用广义逆求解A*M1=B
  M1=(inv(A'*A)*A'*B)';
  % 将M1扩展为12列
  M1(:,12)=1;
  % 将M1转换为3*4的矩阵
  M2=reshape(M1,4,3);
  MB=M2';
  %求解m34
  d=[MB(3,1),MB(3,2),MB(3,3)];
  m34=sqrt(d*d');
  %求解M
  N=MB/m34;
  u16=(MB(1,1)*Xw16+MB(1,2)*Yw16+MB(1,3)*Zw16+MB(1,4))/(MB(3,1)*Xw16+MB(3,2)*Yw16+MB(3,3)*Zw16+1)
  v16=(MB(2,1)*Xw16+MB(2,2)*Yw16+MB(2,3)*Zw16+MB(2,4))/(MB(3,1)*Xw16+MB(3,2)*Yw16+MB(3,3)*Zw16+1)
  %*********************************************8
  %左摄像机标定用数据
format long;
u1=237;
v1=95;
Xw1=0;
Yw1=0;
Zw1=34;

u2=360;
v2=96;
Xw2=40;
Yw2=0;
Zw2=26;

u3=482
v3=100;
Xw3=80;
Yw3=0;
Zw3=18;

u4=602;
v4=97;
Xw4=120;
Yw4=0;
Zw4=26;

u5=236;
v5=213;
Xw5=0;
Yw5=40;
Zw5=34;

u6=356;
v6=215;
Xw6=40;
Yw6=40;
Zw6=30;

u7=480;
v7=218;
Xw7=80;
Yw7=40;
Zw7=22;

u8=603;
v8=220;
Xw8=120;
Yw8=40;
Zw8=10;

u9=239;
v9=333;
Xw9=0;
Yw9=80;
Zw9=26;

u10=355;
v10=337;
Xw10=40;
Yw10=80;
Zw10=30;

u11=477;
v11=338;
Xw11=80;
Yw11=80;
Zw11=22;

u12=600;
v12=340;
Xw12=120;
Yw12=80;
Zw12=10;

u13=248;
v13=444;
Xw13=0;
Yw13=120;
Zw13=14;

u14=362;
v14=449;
Xw14=40;
Yw14=120;
Zw14=14;

u15=473;
v15=458;
Xw15=80;
Yw15=120;
Zw15=26;


Xw16=80;
Yw16=120;
Zw16=26;

A=[Xw1, Yw1, Zw1, 1,  0,  0,   0,   0, -u1*Xw1, -u1*Yw1, -u1*Zw1;
   0,    0,   0,  0, Xw1, Yw1, Zw1, 1, -v1*Xw1, -v1*Yw1, -v1*Zw1;
   Xw2, Yw2, Zw2, 1,  0,  0,   0,   0, -u2*Xw2, -u2*Yw2, -u2*Zw2;
   0,    0,   0,  0, Xw2, Yw2, Zw2, 1, -v2*Xw2, -v2*Yw2, -v2*Zw2;
   Xw3, Yw3, Zw3, 1,  0,  0,   0,   0, -u3*Xw3, -u3*Yw3, -u3*Zw3;
   0,    0,   0,  0, Xw3, Yw3, Zw3, 1, -v3*Xw3, -v3*Yw3, -v3*Zw3;
   Xw4, Yw4, Zw4, 1,  0,  0,   0,   0, -u4*Xw4, -u4*Yw4, -u4*Zw4;
   0,    0,   0,  0, Xw4, Yw4, Zw4, 1, -v4*Xw4, -v4*Yw4, -v4*Zw4;
   Xw5, Yw5, Zw5, 1,  0,  0,   0,   0, -u5*Xw5, -u5*Yw5, -u5*Zw5;
   0,    0,   0,  0, Xw5, Yw5, Zw5, 1, -v5*Xw5, -v5*Yw5, -v5*Zw5;
   Xw6, Yw6, Zw6, 1,  0,  0,   0,   0, -u6*Xw6, -u6*Yw6, -u6*Zw6;
   0,    0,   0,  0, Xw6, Yw6, Zw6, 1, -v6*Xw6, -v6*Yw6, -v6*Zw6;
   Xw7, Yw7, Zw7, 1,  0,  0,   0,   0, -u7*Xw7, -u7*Yw7, -u7*Zw7;
   0,    0,   0,  0, Xw7, Yw7, Zw7, 1, -v7*Xw7, -v7*Yw7, -v7*Zw7;
   Xw8, Yw8, Zw8, 1,  0,  0,   0,   0, -u8*Xw8, -u8*Yw8, -u8*Zw8;
   0,    0,   0,  0, Xw8, Yw8, Zw8, 1, -v8*Xw8, -v8*Yw8, -v8*Zw8;
   Xw9, Yw9, Zw9, 1,  0,  0,   0,   0, -u9*Xw9, -u9*Yw9, -u9*Zw9;
   0,    0,   0,  0, Xw9, Yw9, Zw9, 1, -v9*Xw9, -v9*Yw9, -v9*Zw9;
   Xw10, Yw10, Zw10, 1,  0,  0,   0,   0, -u10*Xw10, -u10*Yw10, -u10*Zw10;
   0,    0,   0,  0, Xw10, Yw10, Zw10, 1, -v10*Xw10, -v10*Yw10, -v10*Zw10;
   
   Xw11, Yw11, Zw11, 1,  0,  0,   0,   0, -u11*Xw11, -u11*Yw11, -u11*Zw11;
   0,    0,   0,  0, Xw11, Yw11, Zw11, 1, -v11*Xw11, -v11*Yw11, -v11*Zw11;
   Xw12, Yw12, Zw12, 1,  0,  0,   0,   0, -u12*Xw12, -u12*Yw12, -u12*Zw12;
   0,    0,   0,  0, Xw12, Yw12, Zw12, 1, -v12*Xw12, -v12*Yw12, -v12*Zw12;
   Xw13, Yw13, Zw13, 1,  0,  0,   0,   0, -u13*Xw13, -u13*Yw13, -u13*Zw13;
   0,    0,   0,  0, Xw13, Yw13, Zw13, 1, -v13*Xw13, -v13*Yw13, -v13*Zw13;
   Xw14, Yw14, Zw14, 1,  0,  0,   0,   0, -u14*Xw14, -u14*Yw14, -u14*Zw14;
   0,    0,   0,  0, Xw14, Yw14, Zw14, 1, -v14*Xw14, -v14*Yw14, -v14*Zw14;
   Xw15, Yw15, Zw15, 1,  0,  0,   0,   0, -u15*Xw15, -u15*Yw15, -u15*Zw15;
   0,    0,   0,  0, Xw15, Yw15, Zw15, 1, -v15*Xw15, -v15*Yw15, -v15*Zw15; 
    ];
  B=[u1,v1,u2,v2,u3,v3,u4,v4,u5,v5,u6,v6,u7,v7,u8,v8,u9,v9, u10,v10,u11,v11,u12,v12,u13,v13,u14,v14,u15,v15]
  B=B';
  % 运用广义逆求解A*M1=B
  M1=(inv(A'*A)*A'*B)';
  % 将M1扩展为12列
  M1(:,12)=1;
  % 将M1转换为3*4的矩阵
  M2=reshape(M1,4,3);
  MB=M2';
  %求解m34
  d=[MB(3,1),MB(3,2),MB(3,3)];
  m34=sqrt(d*d');
  %求解M
  M=MB/m34;
  u16=(MB(1,1)*Xw16+MB(1,2)*Yw16+MB(1,3)*Zw16+MB(1,4))/(MB(3,1)*Xw16+MB(3,2)*Yw16+MB(3,3)*Zw16+1)
  v16=(MB(2,1)*Xw16+MB(2,2)*Yw16+MB(2,3)*Zw16+MB(2,4))/(MB(3,1)*Xw16+MB(3,2)*Yw16+MB(3,3)*Zw16+1)
  %*****************************************
 ul=482
 vl=100
 ur=443
 vr=73

 E=[M(3,1:3)*ul-M(1,1:3);M(3,1:3)*vl-M(2,1:3);N(3,1:3)*ur-N(1,1:3);N(3,1:3)*vr-N(2,1:3);];
 F=[M(1,4)-M(3,4)*ul;M(2,4)-M(3,4)*vl;N(1,4)-N(3,4)*ur;N(2,4)-N(3,4)*vr;];
 P=pinv(E)*F;
 %********************************
  % 理论测定的值 
  % Xw16=80.0
  % Yw16=0.0
  % Zw16=18.0
  %经过Matlab运算得到的试验值
  %X'w16=79.77508164506581
  %Y'w16=0.08068881938200
  %Z'w16=17.76543369869931
  % 精确度计算
  % 绝对误差r1=|80.0-79.78|=0.22  r2=|0.0-0.08|=0.08  r3=|18.0-17.77|=0.23 
  
  % 平均误差计算 
  % 平均绝对误差 r=(r1+r2+r3)/3=0.18
  
 % 总的来说理论值和实验值符合得相当好

⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -