gusucode.com > 基于matlab软件,实现双目视觉原理的摄像机标定,能根据各视场图像求内、外部参数 > 基于matlab软件,实现双目视觉原理的摄像机标定,能根据各视场图像求内、外部参数/TOOLBOX_calib/Compute3D.m
function [Xc,Xp] = Compute3D(xc,xp,R,T,fc,fp,cc,cp,kc,kp,alpha_c,alpha_p); % [Xc,Xp] = Compute3D(xc,xp,R,T,fc,fp,cc,cp,kc,kp,alpha_c,alpha_p); % % Reconstruction of the 3D structure of the striped object. % % Xc : The 3D coordinates of the points in the camera reference frame % Xp : The 3D coordinates of the points in the projector reference frame % % xc, xp: Camera coordinates and projector coordinates from ComputeStripes % R,T : rigid motion from camera to projector: Xp = R*Xc + T % fc,fp : Camera and Projector focal lengths % cc,cp : Camera and Projector center of projection % kc,kp : Camera and Projector distortion factors % alpha_c, alpha_p: skew coefficients for camera and projector % % The set R,T,fc,fp,cc,cp and kc comes from the calibration. % Intel Corporation - Dec. 2003 % (c) Jean-Yves Bouguet if nargin < 12, alpha_p = 0; if nargin < 11, alpha_c = 0; end; end; Np = size(xc,2); xc = normalize_pixel(xc,fc,cc,kc,alpha_c); xp = (xp - cp(1))/fp(1); xp_save = xp; % save the real distorted x - coordinates + alpha'ed if (norm(kp) == 0)&(alpha_p == 0), N_rep = 1; else N_rep = 5; end; % xp is the first entry of the undistorted projector coordinates (iteratively refined) % xc is the complete undistorted camera coordinates for kk = 1:N_rep, R2 = R([1 3],:); if length(T) > 2, Tp = T([1 3]); % The old technique for calibration else Tp = T; % The new technique for calibration (using stripes only) end; % Triangulation: D1 = [-xc(1,:);xc(1,:).*xp(1,:);-xc(2,:);xc(2,:).*xp(1,:);-ones(1,Np);xp(1,:)]; D2 = R2(:)*ones(1,Np); D = sum(D1.*D2); N1 = [-ones(1,Np);xp(1,:)]; N2 = -sum(N1.*(Tp*ones(1,Np))); Z = N2./D; Xc = (ones(3,1)*Z).*[xc;ones(1,Np)]; % reproject on the projetor view, and apply distortion... Xp = R*Xc + T*ones(1,Np); xp_v = [Xp(1,:)./Xp(3,:); Xp(2,:)./Xp(3,:)]; xp_v(1,:) = xp_v(1,:) + alpha_p * xp_v(2,:); xp_dist = apply_distortion(xp_v,kp); %norm(xp_dist(1,:) - xp_save) xp_dist(1,:) = xp_save; xp_v = comp_distortion(xp_dist,kp); xp_v(1,:) = xp_v(1,:) - alpha_p * xp_v(2,:); xp = xp_v(1,:); end;