calib_stereo.m

这是一个很好的标定工具箱

% Calibration of the stereo rig:

load Calib_Results_left;

fc_left = fc;
cc_left = cc;
kc_left = kc;
alpha_c_left = alpha_c;
KK_left = KK;

X_left = [];

for kk = 1:n_ima,
   
   if active_images(kk),
      
      eval(['Xkk = X_' num2str(kk) ';']);
      eval(['omckk = omc_' num2str(kk) ';']);
      eval(['Rckk = Rc_' num2str(kk) ';']);
      eval(['Tckk = Tc_' num2str(kk) ';']);
      
      N = size(Xkk,2);
      
      Xckk = Rckk * Xkk  + Tckk*ones(1,N);
      
      X_left = [X_left Xckk];
      
   end;
   
end;




load Calib_Results_right;

fc_right = fc;
cc_right = cc;
kc_right = kc;
alpha_c_right = alpha_c;
KK_right = KK;

X_right = [];

for kk = 1:n_ima,
   
   if active_images(kk),
      
      eval(['Xkk = X_' num2str(kk) ';']);
      eval(['omckk = omc_' num2str(kk) ';']);
      eval(['Rckk = Rc_' num2str(kk) ';']);
      eval(['Tckk = Tc_' num2str(kk) ';']);
      
      N = size(Xkk,2);
      
      Xckk = Rckk * Xkk  + Tckk*ones(1,N);
      
      X_right = [X_right Xckk];
      
   end;
   
end;



N_points = size(X_left,2);


%%% Find the rigid motion between the two cameras:
%%% X_right = R * X_left + T

A = zeros(3*N_points, 12);

A(1:3:end,1:3:9) = X_left';
A(2:3:end,2:3:9) = X_left';
A(3:3:end,3:3:9) = X_left';

A(1:3:end,10) = ones(N_points,1);
A(2:3:end,11) = ones(N_points,1);
A(3:3:end,12) = ones(N_points,1);

RT = inv(A'*A)*A'*X_right(:);

R = reshape(RT(1:9),3,3);
T = RT(10:12);

[U,S,V] = svd(R);
R = U*V';

om = rodrigues(R);


% Optimize om and T:

for kk = 1:10,
	[Xr2,dXdom,dXdT] = rigid_motion(X_left,om,T);
	
	e = X_right - Xr2;
	J = [dXdom dXdT];
	
	domT = inv(J'*J)*J' * e(:);
	
	om = om + domT(1:3)
	T = T + domT(4:6)
end;

R = rodrigues(om);



% Save the calibration results:

save Calib_Results_stereo  om R T fc_left  cc_left kc_left  alpha_c_left  KK_left  fc_right  cc_right kc_right  alpha_c_right  KK_right