gusucode.com > 基于matlab软件,实现双目视觉原理的摄像机标定,能根据各视场图像求内、外部参数 > 基于matlab软件,实现双目视觉原理的摄像机标定,能根据各视场图像求内、外部参数/TOOLBOX_calib/project_points.m
function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points(X,om,T,f,c,k) %project_points.m % %[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points(X,om,T,f,c,k) % %Projects a 3D structure onto the image plane. % %INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points) % (om,T): Rigid motion parameters between world coordinate frame and camera reference frame % om: rotation vector (3x1 vector); T: translation vector (3x1 vector) % f: camera focal length in units of horizontal and vertical pixel units (2x1 vector) % c: principal point location in pixel units (2x1 vector) % k: Distortion coefficients (radial and tangential) (4x1 vector) % %OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points) % dxpdom: Derivative of xp with respect to om ((2N)x3 matrix) % dxpdT: Derivative of xp with respect to T ((2N)x3 matrix) % dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar) % dxpdc: Derivative of xp with respect to c ((2N)x2 matrix) % dxpdk: Derivative of xp with respect to k ((2N)x4 matrix) % %Definitions: %Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X) %The coordinate vector of P in the camera reference frame is: Xc = R*X + T %where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om); %call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3); %The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z. %call r^2 = a^2 + b^2. %The distorted point coordinates are: xd = [xx;yy] where: % %xx = a * (1 + kc(1)*r^2 + kc(2)*r^4) + 2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2); %yy = b * (1 + kc(1)*r^2 + kc(2)*r^4) + kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b; % %The left terms correspond to radial distortion, the right terms correspond to tangential distortion % %Fianlly, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where: % %xxp = f(1)*xx + c(1) %yyp = f(2)*yy + c(2) % % %NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices % % %Important function called within that program: % %rodrigues.m: Computes the rotation matrix corresponding to a rotation vector % %rigid_motion.m: Computes the rigid motion transformation of a given structure if nargin < 6, k = zeros(4,1); if nargin < 5, c = zeros(2,1); if nargin < 4, f = ones(2,1); if nargin < 3, T = zeros(3,1); if nargin < 2, om = zeros(3,1); if nargin < 1, error('Need at least a 3D structure to project (in project_points.m)'); return; end; end; end; end; end; end; [m,n] = size(X); [Y,dYdom,dYdT] = rigid_motion(X,om,T); inv_Z = 1./Y(3,:); x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ; bb = (-x(1,:) .* inv_Z)'*ones(1,3); cc = (-x(2,:) .* inv_Z)'*ones(1,3); dxdom = zeros(2*n,3); dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:); dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:); dxdT = zeros(2*n,3); dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:); dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:); % Add distortion: r2 = x(1,:).^2 + x(2,:).^2; dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:); dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:); r4 = r2.^2; dr4dom = 2*((r2')*ones(1,3)) .* dr2dom; dr4dT = 2*((r2')*ones(1,3)) .* dr2dT; % Radial distortion: cdist = 1 + k(1) * r2 + k(2) * r4; dcdistdom = k(1) * dr2dom + k(2) * dr4dom; dcdistdT = k(1) * dr2dT+ k(2) * dr4dT; dcdistdk = [ r2' r4' zeros(n,2)]; xd1 = x .* (ones(2,1)*cdist); dxd1dom = zeros(2*n,3); dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom; dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom; coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3)); dxd1dom = dxd1dom + coeff.* dxdom; dxd1dT = zeros(2*n,3); dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT; dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT; dxd1dT = dxd1dT + coeff.* dxdT; dxd1dk = zeros(2*n,4); dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,4)) .* dcdistdk; dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,4)) .* dcdistdk; % tangential distortion: a1 = 2.*x(1,:).*x(2,:); a2 = r2 + 2*x(1,:).^2; a3 = r2 + 2*x(2,:).^2; delta_x = [k(3)*a1 + k(4)*a2 ; k(3) * a3 + k(4)*a1]; ddelta_xdx = zeros(2*n,2*n); aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3); bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3); cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3); ddelta_xdom = zeros(2*n,3); ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:); ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:); ddelta_xdT = zeros(2*n,3); ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:); ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:); ddelta_xdk = zeros(2*n,4); ddelta_xdk(1:2:end,3) = a1'; ddelta_xdk(1:2:end,4) = a2'; ddelta_xdk(2:2:end,3) = a3'; ddelta_xdk(2:2:end,4) = a1'; xd2 = xd1 + delta_x; dxd2dom = dxd1dom + ddelta_xdom ; dxd2dT = dxd1dT + ddelta_xdT; dxd2dk = dxd1dk + ddelta_xdk ; % Pixel coordinates: if length(f)>1, xp = xd2 .* (f * ones(1,n)) + c*ones(1,n); coeff = reshape(f*ones(1,n),2*n,1); dxpdom = (coeff*ones(1,3)) .* dxd2dom; dxpdT = (coeff*ones(1,3)) .* dxd2dT; dxpdk = (coeff*ones(1,5)) .* dxd2dk; dxpdf = zeros(2*n,2); dxpdf(1:2:end,1) = xd2(1,:)'; dxpdf(2:2:end,2) = xd2(2,:)'; else xp = f * xd2 + c*ones(1,n); dxpdom = f * dxd2dom; dxpdT = f * dxd2dT; dxpdk = f * dxd2dk; dxpdf = xd2(:); end; dxpdc = zeros(2*n,2); dxpdc(1:2:end,1) = ones(n,1); dxpdc(2:2:end,2) = ones(n,1); return; % Test of the Jacobians: n = 10; X = 10*randn(3,n); om = randn(3,1); T = [10*randn(2,1);40]; f = 1000*rand(2,1); c = 1000*randn(2,1); k = 0.5*randn(4,1); [x,dxdom,dxdT,dxdf,dxdc,dxdk] = project_points(X,om,T,f,c,k); % Test on om: OK dom = 0.000000001 * norm(om)*randn(3,1); om2 = om + dom; [x2] = project_points(X,om2,T,f,c,k); x_pred = x + reshape(dxdom * dom,2,n); norm(x2-x)/norm(x2 - x_pred) % Test on T: OK!! dT = 0.0001 * norm(T)*randn(3,1); T2 = T + dT; [x2] = project_points(X,om,T2,f,c,k); x_pred = x + reshape(dxdT * dT,2,n); norm(x2-x)/norm(x2 - x_pred) % Test on f: OK!! df = 0.001 * norm(f)*randn(2,1); f2 = f + df; [x2] = project_points(X,om,T,f2,c,k); x_pred = x + reshape(dxdf * df,2,n); norm(x2-x)/norm(x2 - x_pred) % Test on c: OK!! dc = 0.01 * norm(c)*randn(2,1); c2 = c + dc; [x2] = project_points(X,om,T,f,c2,k); x_pred = x + reshape(dxdc * dc,2,n); norm(x2-x)/norm(x2 - x_pred) % Test on k: OK!! dk = 0.001 * norm(4)*randn(4,1); k2 = k + dk; [x2] = project_points(X,om,T,f,c,k2); x_pred = x + reshape(dxdk * dk,2,n); norm(x2-x)/norm(x2 - x_pred)