% [New_A,New_R_t]=My_Tsai(Imag_corner,World_points)%
% **********************************************************************************************
% ******* Calibrating a Camera Using a Monoview Coplanar Set of Points *******
% **********************************************************************************************
% 8/5/2013 Joseph Chen
% 1240803839@qq.com
%
% Note: Xf, Yf, xw, yw, zw are all column vectors
%
% (xw, yw, zw) is the 3D coordinate of the object point P in the 3D world coordinate system
% (x, y, z) is ths 3D coordinate of the object point P in the 3D camera coordinate system
% (X, Y) is the image coordinate system centered at Oi where is the intersection of the optical center axis z and the front plane
% (Xu, Yu) is the image coordinate of (x, y, z) if a perfect pinhole camera model is used
% Xu = f * x / z (4a)
% Yu = f * y / z (4b)
% (Xd, Yd) is the actual image coordinate which differs from (Xu, Yu) due to lens distortion
% (Xf, Yf) is the coordinate used in the computer, is the number of pixels for the discrete image in the frame memory
% R is the 3*3 rotation matrix
% = [r1, r2, r3; r4, r5, r6; r7, r8, r9]; (2)
% [x, y, z]' = R * [xw, yw, zw]' + T (1)
% T is the translation vector
% = [Tx, Ty, Tz]' (3)
% f is the effective focal length
% Dx = Xd*( k1*r^2 + k2*r^4 + ... ) P327
% Xd+Dx=Xu (5a)
% Dy = Yd*( k1*r^2 + k2*r^4 + ... ) P327
% Yd+Dy=Yu (5b)
% r = (Xd^2 + Yd^2)^(0.5) P327
% k1 is the distortion coeffient
% Xf = sx * dxp^(-1) * Xd + Cx (6a)
% Yf = dy^(-1) * Yd + Cy (6b)
% dxp = dx * Ncx / Nfx (6d)
% dx is the center to center distance between adjacent sensor elements in X (scan line) diretion
% dy is the center to center distance between adjacent CCD sensor in the Y direction
% Ncx is the number of sensor elements in the X direction
% Nfx is the number of pixels in a line as sampled by the computer
% sx is the uncertainty image scale factor
% X = (Xd * Nfx) / (dx * Ncx) P328
% X = Xf - Cx P328
% Y = Yf - Cy P328
% sx^(-1)*dxp*X + sz^(-1)*dxp*X*k1*r^2 = f*x/z (7a)
% dxp*Y + dy*Y*k1*r^2 = f*y/z (7b)
% r = ( ( sx^(-1)*dxp*X )^2 + (dx*Y)^2 )^(0.5)
% sx^(-1)*dxp*X + sx^(-1)*dxp*X*k1*r^2 = f*(r1*xw + r2*yw + r3*zw +
% Tx) / (r7*xw + r8*yw + r9*zw +Tz) (8a)
% dy*Y + dy*Y*k1*r^2 = f*(r1*xw + r2*yw + r3*zw +
% Tx) / (r7*xw + r8*yw + r9*zw +Tz) (8b)
% Since the calibration points are on a common plane, the (xw, yw, zw) coordinate system can be chosen such that zw=0 and the
% corigin is not lose to the center of the view or y axis of the camera coordinate system. Since the (xw, yw, zw) is user-defined
% and the origin is arbitrary, it is no problem setting the origin of (xw, yw, zw) to be out of the field of view and not close
% to the y axis. the purpose for the latter is to make sure that Ty is not exactly zero.
%
% REF: "A versatile camera calibration technique for high-accuracy 3D machine
% vision metrology using off-the-shelf TV cameras and lens"
% R.Y. Tsai, IEEE Trans R&A RA-3, No.4, Aug 1987, pp 323-344.
%
function [New_A,New_R_t,k1]=My_Tsai_Fcn(Imag_corner,World_points,u0,v0)
if ~ismatrix(Imag_corner) || ~ismatrix(World_points)|| size(World_points,1)~=size(Imag_corner,1)||size(Imag_corner,2)~=2||size(World_points,2)~=3
error('MATLAB:square','Matrix must be square.');
end
Cx=u0;
Cy=v0;
X=Imag_corner;
x=World_points;
Xf=X(:,1);
Yf=X(:,2);
xw=x(:,1);
yw=x(:,2);
[M,N]=size(x);
% zw=zeros(M,1);
zw=x(:,3);
zuobian=[];
youbian=[];
My_Parameter=1;
for jj=0:length(x)-1
tmp=[xw(1+jj*My_Parameter)*(Yf(1+jj*My_Parameter)-Cy),yw(1+jj*My_Parameter)*(Yf(1+jj*My_Parameter)-Cy),zw(1+jj*My_Parameter)...
*(Yf(1+jj*My_Parameter)-Cy),(Yf(1+jj*My_Parameter)-Cy),-xw(1+jj*My_Parameter)*...
(Xf(1+jj*My_Parameter)-Cx),-yw(1+jj*My_Parameter)*(Xf(1+jj*My_Parameter)-Cx),-zw(1+jj*My_Parameter)*(Xf(1+jj*My_Parameter)-Cx)];
zuobian=[zuobian;tmp];
tmp2=Xf(1+jj*My_Parameter)-Cx;
youbian=[youbian;tmp2];
end
Rank_zuobian=rank(zuobian)
R_t=pinv(zuobian)*youbian;
clear zuobian youbian;
%% 求解f和t3
R_t=[R_t;1];
R_t=R_t/(sqrt(sum(R_t(5:7).^2)));
s=sqrt(sum(R_t(1:3).^2));
R_t(1:4)=R_t(1:4)/s;
r3=cross(R_t(1:3),R_t(5:7));
New_R=[R_t(1:3)';R_t(5:7)';r3'];
t1=R_t(4);
t2=R_t(8);
zuobian=[];
youbian=[];
My_Parameter=1;
for i=1:length(x)
tmp=[s*(New_R(1,:)*x(i,:)'+t1),-X(i,1)];
zuobian=[zuobian;tmp];
tmp2=X(i,1)*(New_R(3,:)*x(i,:)');
youbian=[youbian;tmp2];
tmp=[(New_R(2,:)*x(i,:)'+t2),-X(i,2)];
zuobian=[zuobian;tmp];
tmp2=X(i,2)*(New_R(3,:)*x(i,:)');
youbian=[youbian;tmp2];
end
f_t3=pinv(zuobian)*youbian;
t3=f_t3(2);
f=f_t3(1);
New_R_t=[New_R,[t1,t2,t3]'];
New_A=[f*s,0,Cx
0, f,Cy
0, 0,1];
if nargout<1
disp('My_Tsai_Fcn径向一致约束（before getting k1）: 旋转平移矩阵为： \n');
New_R_t
disp('My_Tsai_Fcn径向一致约束（before getting k1）: 内参数矩阵为： \n');
New_A
end
% 2) Stage 2 --- Compute Effective Focal Length, Distortion Coefficients, and z Position:
% d) Compute an approximation of f and Tz by ignoring lens distortion:
% Compute the exactly solution for f, Tz, k1:
params_const = [New_R(1,1:3),New_R(2,1:3),New_R(3,1:3), s, t1,t2];
params = [f, t3, 0]; % add initial guess for k1
[x,fval,exitflag,output]= fminsearch( @Tsai_8b, params, [], params_const,u0,v0, xw, yw, zw, Xf, Yf);
x= fminsearch( @Tsai_8b, params, [], params_const,u0,v0, xw, yw, zw, Xf, Yf);
f = x(1);
t3 = x(2);
k1 = x(3);
New_R_t(3,4)=t3;
New_A(1,1)=f*s;
New_A(2,2)=f;
if nargout<1
disp('My_Tsai_Fcn径向一致约束（after getting k1）: 旋转平移矩阵为： \n');
New_R_t
disp('My_Tsai_Fcn径向一致约束（after getting k1）: 内参数矩阵为： \n');
New_A
disp('My_Tsai_Fcn径向一致约束（after getting k1）: 畸变参数k1为： \n');
k1
end
% fval the value of the objective function fun at the solution x.
% fval
% exitflag that describes the exit condition of fminsearch
% >0 Indicates that the function converged to a solution x.
% 0 Indicates that the maximum number of function evaluations was exceeded.
% <0 Indicates that the function did not converge to a solution.
% exitflag
% output that contains information about the optimization
% output.algorithmThe algorithm used
% output.funcCountThe number of function evaluations
% output.iterationsThe number of iterations taken
% output
% f = Tsai_8b(params, params_const, u0,v0,xw, yw, zw, X, Y)
%
% **********************************************************************************************
% ******* Calibrating a Camera Using a Monoview Coplanar Set of Points *******
% **********************************************************************************************
% 6/2004 Simon Wan
% sim
- 1
- 2
- 3
- 4
- 5
- 6
前往页