mywork2.m

fqfafafa fhahhhjhgjhgjkjkkhjkkh

clear
clc
close  all
%左右摄像机分别标定，先标定左摄像机，然后再标定右摄像机
XW_L=[270 270 270 270 270 270 270 270 270 270 270 270 270 270 240 240 240 240 240 240 240 240 240 240 240 240 240 240 210 210 210 210 210 210 210 210 210 210 210 210 210 180 180 180 180 180 180 180 180 180];
YW_L=[210 180 150 120 90 60 0 0 0 30 0 0 0 0 210 180 150 120 90 0 0 60 0 0 30 0 0 0 210 180 150 120 90 0 0 0 60 0 0 30 0 180 150 120 0 90 0 0 60 0];
ZW_L=[0 0 0 0 0 0 180 150 120 0 90 60 30 0 0 0 0 0 0 180 150 0 120 90 0 60 30 0 0 0 0 0 0 180 150 120 0 90 60 0 0 0 0 0 180 0 150 120 0 90];
u_L=[376 380 385 389 394 399 402 403 404 405 406 407 408 410 419 424 430 437 443 445 449 449 451 453 455 457 459 462 468 471 478 484 492 493 494 498 500 502 506 508 517 518 525 534 536 542 543 546 550 553];
v_L=[465 436 400 369 330 296 43 63 92 253 117 149 177 214 468 438 406 371 335 45 69 298 95 121 258 152 181 215 471 442 408 375 339 49 71 97 302 123 152 262 218 444 413 378 51 342 75 99 305 129];
n=1;
for m=1:2:2*length(XW_L)-1
        S_L(m,1)=XW_L(n); S_L(m,2)=YW_L(n); S_L(m,3)=ZW_L(n); S_L(m,4)=1;  S_L(m,5)=0; S_L(m,6)=0; S_L(m,7)=0; S_L(m,8)=0;
        S_L(m,9)=-u_L(n)*XW_L(n); S_L(m,10)=-u_L(n)*YW_L(n); S_L(m,11)=-u_L(n)*ZW_L(n);
        
        S_L(m+1,1)=0; S_L(m+1,2)=0; S_L(m+1,3)=0; S_L(m+1,4)=0;  S_L(m+1,5)=XW_L(n); S_L(m+1,6)=YW_L(n); S_L(m+1,7)=ZW_L(n); S_L(m+1,8)=1;
        S_L(m+1,9)=-v_L(n)*XW_L(n); S_L(m+1,10)=-v_L(n)*YW_L(n); S_L(m+1,11)=-v_L(n)*ZW_L(n);
        n=n+1;
end
U_L=zeros(2*length(XW_L));
j=1;
for i=1:2:2*length(XW_L)
    U_L(i)=u_L(j);
    U_L(i+1)=v_L(j);
    j=j+1;
end
% A=S'*S;
% [V1,D1]=eig(A);
% h1=V1(:,1)/V1(9,1)%单应性矩阵
% S*V1(:,1)
% S*h1
n_L=(inv(S_L'*S_L))*S_L'*U_L;
m_L=[n_L(1),n_L(2),n_L(3),n_L(4);
   n_L(5),n_L(6),n_L(7),n_L(8);
   n_L(9),n_L(10),n_L(11),1]
%         m1_L=[n_L(1),n_L(2),n_L(3)]';
%         m2_L=[n_L(5),n_L(6),n_L(7)]';
%         m3_L=[n_L(9),n_L(10),n_L(11)]';
%         % m3_mod_L=sqrt(n_L(9)^2+n_L(10)^2+n_L(11)^2);
%         m3_mod_L=norm(m3_L);
%         m_L(3,4)=1/m3_mod_L;
%         % m_L=m_L(3,4)*m_L;
%         %左摄像机参数求解
%         r3_L=m_L(3,4)*m3_L;
%         u0_L=m_L(3,4)^2*m1_L'*m3_L;
%         v0_L=m_L(3,4)^2*m2_L'*m3_L;
%         ax_L=m_L(3,4)^2*norm(cross(m1_L,m3_L));
%         ay_L=m_L(3,4)^2*norm(cross(m2_L,m3_L));
%         r1_L=(m_L(3,4)/ax_L)*(m1_L-u0_L*m3_L);
%         r2_L=(m_L(3,4)/ay_L)*(m2_L-v0_L*m3_L);
%         tz_L=m_L(3,4);
%         tx_L=(m_L(3,4)/ax_L)*(m_L(1,4)-u0_L);
%         ty_L=(m_L(3,4)/ay_L)*(m_L(2,4)-v0_L);
%         neicanshu_L=[ax_L,0,u0_L;0,ay_L,v0_L;0,0,1]
%         waicanshu_L=[r1_L',tx_L;r2_L',ty_L;r3_L',tz_L]

%开始对右摄像机进行标定
XW_R=[210 180 180 180 180 150 180 150 150 150 150 120 150 120 120 120 120 120 120 120 120 120 120 90 90 90 90 90 90 90 90 90 90 90 90 90 60 60 60 60 60 60 60 60 60 60 60 60 60 60 30];
YW_R=[0 60 90 120 150 0 0 90 120 180 0 0 0 30 60 0 90 120 0 150 180 210 0 0 30 0 60 0 90 0 120 0 180 0 210 0 0 30 0 60 0 90 120 0 150 0 180 210 0 0 0];
ZW_R=[180 0 0 0 0 0 180 0 0 0 150 0 180 0 0 60 0 0 90 0 0 0 180 0 0 30 0 60 0 90 0 120 0 150 0 180 0 0 30 0 60 0 0 90 0 120 0 0 150 180 0];
u_R=[9 10 18 26 34 44 50 66 73 87 90 94 97 101 109 113 115 121 122 127 133 136 140 149 152 155 159 163 165 169 169 174 179 180 184 188 200 205 207 208 213 214 218 219 221 222 224 227 229 231 257];
v_R=[12 265 301 336 370 181 12 303 338 404 34 182 9 226 265 117 302 338 87 372 404 432 11 182 225 148 265 117 302 88 337 62 403 35 431 11 183 226 148 264 117 302 337 87 370 62 402 431 33 11 183];
S_R=zeros(length(XW_R),11);
n=1;
for m=1:2:2*length(XW_R)-1
        S_R(m,1)=XW_R(n); S_R(m,2)=YW_R(n); S_R(m,3)=ZW_R(n); S_R(m,4)=1;  S_R(m,5)=0; S_R(m,6)=0; S_R(m,7)=0; S_R(m,8)=0;
        S_R(m,9)=-u_R(n)*XW_R(n); S_R(m,10)=-u_R(n)*YW_R(n); S_R(m,11)=-u_R(n)*ZW_R(n);
        
        S_R(m+1,1)=0; S_R(m+1,2)=0; S_R(m+1,3)=0; S_R(m+1,4)=0;  S_R(m+1,5)=XW_R(n); S_R(m+1,6)=YW_R(n); S_R(m+1,7)=ZW_R(n); S_R(m+1,8)=1;
        S_R(m+1,9)=-v_R(n)*XW_R(n); S_R(m+1,10)=-v_R(n)*YW_R(n); S_R(m+1,11)=-v_R(n)*ZW_R(n);
        n=n+1;
end
U_R=zeros(2*length(XW_R));
j=1;
for i=1:2:2*length(XW_R)-1
    U_R(i)=u_R(j);
    U_R(i+1)=v_R(j);
    j=j+1;
end
n_R=(inv(S_R'*S_R))*S_R'*U_R;
m_R=[n_R(1),n_R(2),n_R(3),n_R(4);
   n_R(5),n_R(6),n_R(7),n_R(8);
   n_R(9),n_R(10),n_R(11),1]
%         m1_R=[n_R(1),n_R(2),n_R(3)]';
%         m2_R=[n_R(5),n_R(6),n_R(7)]';
%         m3_R=[n_R(9),n_R(10),n_R(11)]';
%         m3_mod_R=sqrt(n_R(9)^2+n_R(10)^2+n_R(11)^2);
%         m_R(3,4)=1/m3_mod_R;
% %         m_R=m_R(3,4)*m_R;
%         %右摄像机参数求解
%         r3_R=m_R(3,4)*m3_R;
%         u0_R=m_R(3,4)^2*m1_R'*m3_R;
%         v0_R=m_R(3,4)^2*m2_R'*m3_R;
%         ax_R=m_R(3,4)^2*norm(cross(m1_R,m3_R));
%         ay_R=m_R(3,4)^2*norm(cross(m2_R,m3_R));
%         r1_R=(m_R(3,4)/ax_R)*(m1_R-u0_R*m3_R);
%         r2_R=(m_R(3,4)/ay_R)*(m2_R-v0_R*m3_R);
%         tz_R=m_R(3,4);
%         tx_R=(m_R(3,4)/ax_R)*(m_R(1,4)-u0_R);
%         ty_R=(m_R(3,4)/ay_R)*(m_R(2,4)-v0_R);
%         neicanshu_R=[ax_R,0,u0_R;0,ay_R,v0_R;0,0,1]
%         waicanshu_R=[r1_R',tx_R;r2_R',ty_R;r3_R',tz_R]
%         % %接下来进行两摄像机之间的相对位置标定
%         R_L=[r1_L';r2_L';r3_L'];
%         T_L=[tx_L;ty_L;tz_L];
%         R_R=[r1_R';r2_R';r3_R'];
%         T_R=[tx_R;ty_R;tz_R];
%         R=R_L*inv(R_R);
%         T=T_L-inv(R_R)*T_R;
%         M_L=[r1_L',tx_L;r2_L',ty_L;r3_L',tz_L;0 0 0 1];
%         M_R=[r1_R',tx_R;r2_R',ty_R;r3_R',tz_R;0 0 0 1];
%         M=M_L*inv(M_R);% M中含有旋转矩阵和平移矩阵
%接下来进行三维重建
u_L1=208;
v_L1=489;
u_R1=156;
v_R1=54;
u_L2=202;
v_L2=493;
u_R2=150;
v_R2=60;
% X=[120 120 150 120 180 150 120 120 120 150 180 150];
% Y=[30 60 30	0 0	0 0	90 120 60 90 0];
% Z=[0 0	0 60 180 150 90	0 0	0 0	0];
% coordinate1=zeros(3,12);
% for i=1:12
A1=[u_L1*m_L(3,1)-m_L(1,1) u_L1*m_L(3,2)-m_L(1,2) u_L1*m_L(3,3)-m_L(1,3);
    v_L1*m_L(3,1)-m_L(2,1) v_L1*m_L(3,2)-m_L(2,2) v_L1*m_L(3,3)-m_L(2,3);
    u_R1*m_R(3,1)-m_R(1,1) u_R1*m_R(3,2)-m_R(1,2) u_R1*m_R(3,3)-m_R(1,3);
    v_R1*m_R(3,1)-m_R(2,1) v_R1*m_R(3,2)-m_R(2,2) v_R1*m_R(3,3)-m_R(2,3);
   ];
b1=[m_L(1,4)-u_L1*m_L(3,4) m_L(2,4)-v_L1*m_L(3,4) m_R(1,4)-u_R1*m_R(3,4) m_R(2,4)-v_R1*m_R(3,4)]';

coordinate1=(inv(A1'*A1))*A1'*b1

A2=[u_L2*m_L(3,1)-m_L(1,1) u_L2*m_L(3,2)-m_L(1,2) u_L2*m_L(3,3)-m_L(1,3);
    v_L2*m_L(3,1)-m_L(2,1) v_L2*m_L(3,2)-m_L(2,2) v_L2*m_L(3,3)-m_L(2,3);
    u_R2*m_R(3,1)-m_R(1,1) u_R2*m_R(3,2)-m_R(1,2) u_R2*m_R(3,3)-m_R(1,3);
    v_R2*m_R(3,1)-m_R(2,1) v_R2*m_R(3,2)-m_R(2,2) v_R2*m_R(3,3)-m_R(2,3);
   ];
b2=[m_L(1,4)-u_L2*m_L(3,4) m_L(2,4)-v_L2*m_L(3,4) m_R(1,4)-u_R2*m_R(3,4) m_R(2,4)-v_R2*m_R(3,4)]';
% coordinate2=zeros(3,12);
coordinate2=(inv(A2'*A2))*A2'*b2
% end
% error_x=coordinate1(1,:)-X;
% error_y=coordinate1(2,:)-Y;
% error_z=coordinate1(3,:)-Z;
% num=1:12;
% plot(num,error_x,'r-');
% hold on
% plot(num,error_y,'b-');
% hold on
% plot(num,error_z,'g-');
% title('误差曲线');
% legend('\itx方向误差','\ity方向误差','\itz方向误差');
% 目标相对位置
% position=coordinate1-coordinate2
% POS=norm(position)