1  %=======================================================================


2  % Copyright 20082014, LEGI UMR 5519 / CNRS UJF GINP, Grenoble, France


3  % http://www.legi.grenobleinp.fr


4  % Joel.Sommeria  Joel.Sommeria (A) legi.cnrs.fr


5  %


6  % This file is part of the toolbox UVMAT.


7  %


8  % UVMAT is free software; you can redistribute it and/or modify


9  % it under the terms of the GNU General Public License as published


10  % by the Free Software Foundation; either version 2 of the license,


11  % or (at your option) any later version.


12  %


13  % UVMAT is distributed in the hope that it will be useful,


14  % but WITHOUT ANY WARRANTY; without even the implied warranty of


15  % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the


16  % GNU General Public License (see LICENSE.txt) for more details.


17  %=======================================================================


18 


19  function [xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk,dxpdalpha] = project_points2(X,om,T,f,c,k,alpha)


20 


21  %project_points2.m


22  %


23  %[xp,dxpdom,dxpdT,dxpdf,dxpdc,dxpdk] = project_points2(X,om,T,f,c,k,alpha)


24  %


25  %Projects a 3D structure onto the image plane.


26  %


27  %INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points)


28  % (om,T): Rigid motion parameters between world coordinate frame and camera reference frame


29  % om: rotation vector (3x1 vector); T: translation vector (3x1 vector)


30  % f: camera focal length in units of horizontal and vertical pixel units (2x1 vector)


31  % c: principal point location in pixel units (2x1 vector)


32  % k: Distortion coefficients (radial and tangential) (4x1 vector)


33  % alpha: Skew coefficient between x and y pixel (alpha = 0 <=> square pixels)


34  %


35  %OUTPUT: xp: Projected pixel coordinates (2xN matrix for N points)


36  % dxpdom: Derivative of xp with respect to om ((2N)x3 matrix)


37  % dxpdT: Derivative of xp with respect to T ((2N)x3 matrix)


38  % dxpdf: Derivative of xp with respect to f ((2N)x2 matrix if f is 2x1, or (2N)x1 matrix is f is a scalar)


39  % dxpdc: Derivative of xp with respect to c ((2N)x2 matrix)


40  % dxpdk: Derivative of xp with respect to k ((2N)x4 matrix)


41  %


42  %Definitions:


43  %Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)


44  %The coordinate vector of P in the camera reference frame is: Xc = R*X + T


45  %where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);


46  %call x, y and z the 3 coordinates of Xc: x = Xc(1); y = Xc(2); z = Xc(3);


47  %The pinehole projection coordinates of P is [a;b] where a=x/z and b=y/z.


48  %call r^2 = a^2 + b^2.


49  %The distorted point coordinates are: xd = [xx;yy] where:


50  %


51  %xx = a * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6) + 2*kc(3)*a*b + kc(4)*(r^2 + 2*a^2);


52  %yy = b * (1 + kc(1)*r^2 + kc(2)*r^4 + kc(5)*r^6) + kc(3)*(r^2 + 2*b^2) + 2*kc(4)*a*b;


53  %


54  %The left terms correspond to radial distortion (6th degree), the right terms correspond to tangential distortion


55  %


56  %Finally, convertion into pixel coordinates: The final pixel coordinates vector xp=[xxp;yyp] where:


57  %


58  %xxp = f(1)*(xx + alpha*yy) + c(1)


59  %yyp = f(2)*yy + c(2)


60  %


61  %


62  %NOTE: About 90 percent of the code takes care fo computing the Jacobian matrices


63  %


64  %


65  %Important function called within that program:


66  %


67  %rodrigues.m: Computes the rotation matrix corresponding to a rotation vector


68  %


69  %rigid_motion.m: Computes the rigid motion transformation of a given structure


70 


71 


72  if nargin < 7,


73  alpha = 0;


74  if nargin < 6,


75  k = zeros(5,1);


76  if nargin < 5,


77  c = zeros(2,1);


78  if nargin < 4,


79  f = ones(2,1);


80  if nargin < 3,


81  T = zeros(3,1);


82  if nargin < 2,


83  om = zeros(3,1);


84  if nargin < 1,


85  error('Need at least a 3D structure to project (in project_points.m)');


86  return;


87  end;


88  end;


89  end;


90  end;


91  end;


92  end;


93  end;


94 


95 


96  [m,n] = size(X);


97 


98  if nargout > 1,


99  [Y,dYdom,dYdT] = rigid_motion(X,om,T);


100  else


101  Y = rigid_motion(X,om,T);


102  end;


103 


104 


105  inv_Z = 1./Y(3,:);


106 


107  x = (Y(1:2,:) .* (ones(2,1) * inv_Z)) ;


108 


109 


110  bb = (x(1,:) .* inv_Z)'*ones(1,3);


111  cc = (x(2,:) .* inv_Z)'*ones(1,3);


112 


113  if nargout > 1,


114  dxdom = zeros(2*n,3);


115  dxdom(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(1:3:end,:) + bb .* dYdom(3:3:end,:);


116  dxdom(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdom(2:3:end,:) + cc .* dYdom(3:3:end,:);


117 


118  dxdT = zeros(2*n,3);


119  dxdT(1:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(1:3:end,:) + bb .* dYdT(3:3:end,:);


120  dxdT(2:2:end,:) = ((inv_Z')*ones(1,3)) .* dYdT(2:3:end,:) + cc .* dYdT(3:3:end,:);


121  end;


122 


123 


124  % Add distortion:


125 


126  r2 = x(1,:).^2 + x(2,:).^2;


127 


128  if nargout > 1,


129  dr2dom = 2*((x(1,:)')*ones(1,3)) .* dxdom(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdom(2:2:end,:);


130  dr2dT = 2*((x(1,:)')*ones(1,3)) .* dxdT(1:2:end,:) + 2*((x(2,:)')*ones(1,3)) .* dxdT(2:2:end,:);


131  end;


132 


133 


134  r4 = r2.^2;


135 


136  if nargout > 1,


137  dr4dom = 2*((r2')*ones(1,3)) .* dr2dom;


138  dr4dT = 2*((r2')*ones(1,3)) .* dr2dT;


139  end


140 


141  r6 = r2.^3;


142 


143  if nargout > 1,


144  dr6dom = 3*((r2'.^2)*ones(1,3)) .* dr2dom;


145  dr6dT = 3*((r2'.^2)*ones(1,3)) .* dr2dT;


146  end;


147 


148  % Radial distortion:


149 


150  cdist = 1 + k(1) * r2 + k(2) * r4 + k(5) * r6;


151 


152  if nargout > 1,


153  dcdistdom = k(1) * dr2dom + k(2) * dr4dom + k(5) * dr6dom;


154  dcdistdT = k(1) * dr2dT + k(2) * dr4dT + k(5) * dr6dT;


155  dcdistdk = [ r2' r4' zeros(n,2) r6'];


156  end;


157 


158  xd1 = x .* (ones(2,1)*cdist);


159 


160  if nargout > 1,


161  dxd1dom = zeros(2*n,3);


162  dxd1dom(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdom;


163  dxd1dom(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdom;


164  coeff = (reshape([cdist;cdist],2*n,1)*ones(1,3));


165  dxd1dom = dxd1dom + coeff.* dxdom;


166 


167  dxd1dT = zeros(2*n,3);


168  dxd1dT(1:2:end,:) = (x(1,:)'*ones(1,3)) .* dcdistdT;


169  dxd1dT(2:2:end,:) = (x(2,:)'*ones(1,3)) .* dcdistdT;


170  dxd1dT = dxd1dT + coeff.* dxdT;


171 


172  dxd1dk = zeros(2*n,5);


173  dxd1dk(1:2:end,:) = (x(1,:)'*ones(1,5)) .* dcdistdk;


174  dxd1dk(2:2:end,:) = (x(2,:)'*ones(1,5)) .* dcdistdk;


175  end;


176 


177 


178  % tangential distortion:


179 


180  a1 = 2.*x(1,:).*x(2,:);


181  a2 = r2 + 2*x(1,:).^2;


182  a3 = r2 + 2*x(2,:).^2;


183 


184  delta_x = [k(3)*a1 + k(4)*a2 ;


185  k(3) * a3 + k(4)*a1];


186 


187 


188  %ddelta_xdx = zeros(2*n,2*n);


189  aa = (2*k(3)*x(2,:)+6*k(4)*x(1,:))'*ones(1,3);


190  bb = (2*k(3)*x(1,:)+2*k(4)*x(2,:))'*ones(1,3);


191  cc = (6*k(3)*x(2,:)+2*k(4)*x(1,:))'*ones(1,3);


192 


193  if nargout > 1,


194  ddelta_xdom = zeros(2*n,3);


195  ddelta_xdom(1:2:end,:) = aa .* dxdom(1:2:end,:) + bb .* dxdom(2:2:end,:);


196  ddelta_xdom(2:2:end,:) = bb .* dxdom(1:2:end,:) + cc .* dxdom(2:2:end,:);


197 


198  ddelta_xdT = zeros(2*n,3);


199  ddelta_xdT(1:2:end,:) = aa .* dxdT(1:2:end,:) + bb .* dxdT(2:2:end,:);


200  ddelta_xdT(2:2:end,:) = bb .* dxdT(1:2:end,:) + cc .* dxdT(2:2:end,:);


201 


202  ddelta_xdk = zeros(2*n,5);


203  ddelta_xdk(1:2:end,3) = a1';


204  ddelta_xdk(1:2:end,4) = a2';


205  ddelta_xdk(2:2:end,3) = a3';


206  ddelta_xdk(2:2:end,4) = a1';


207  end;


208 


209 


210  xd2 = xd1 + delta_x;


211 


212  if nargout > 1,


213  dxd2dom = dxd1dom + ddelta_xdom ;


214  dxd2dT = dxd1dT + ddelta_xdT;


215  dxd2dk = dxd1dk + ddelta_xdk ;


216  end;


217 


218 


219  % Add Skew:


220 


221  xd3 = [xd2(1,:) + alpha*xd2(2,:);xd2(2,:)];


222 


223  % Compute: dxd3dom, dxd3dT, dxd3dk, dxd3dalpha


224  if nargout > 1,


225  dxd3dom = zeros(2*n,3);


226  dxd3dom(1:2:2*n,:) = dxd2dom(1:2:2*n,:) + alpha*dxd2dom(2:2:2*n,:);


227  dxd3dom(2:2:2*n,:) = dxd2dom(2:2:2*n,:);


228  dxd3dT = zeros(2*n,3);


229  dxd3dT(1:2:2*n,:) = dxd2dT(1:2:2*n,:) + alpha*dxd2dT(2:2:2*n,:);


230  dxd3dT(2:2:2*n,:) = dxd2dT(2:2:2*n,:);


231  dxd3dk = zeros(2*n,5);


232  dxd3dk(1:2:2*n,:) = dxd2dk(1:2:2*n,:) + alpha*dxd2dk(2:2:2*n,:);


233  dxd3dk(2:2:2*n,:) = dxd2dk(2:2:2*n,:);


234  dxd3dalpha = zeros(2*n,1);


235  dxd3dalpha(1:2:2*n,:) = xd2(2,:)';


236  end;


237 


238 


239 


240  % Pixel coordinates:


241  if length(f)>1,


242  xp = xd3 .* (f * ones(1,n)) + c*ones(1,n);


243  if nargout > 1,


244  coeff = reshape(f*ones(1,n),2*n,1);


245  dxpdom = (coeff*ones(1,3)) .* dxd3dom;


246  dxpdT = (coeff*ones(1,3)) .* dxd3dT;


247  dxpdk = (coeff*ones(1,5)) .* dxd3dk;


248  dxpdalpha = (coeff) .* dxd3dalpha;


249  dxpdf = zeros(2*n,2);


250  dxpdf(1:2:end,1) = xd3(1,:)';


251  dxpdf(2:2:end,2) = xd3(2,:)';


252  end;


253  else


254  xp = f * xd3 + c*ones(1,n);


255  if nargout > 1,


256  dxpdom = f * dxd3dom;


257  dxpdT = f * dxd3dT;


258  dxpdk = f * dxd3dk;


259  dxpdalpha = f .* dxd3dalpha;


260  dxpdf = xd3(:);


261  end;


262  end;


263 


264  if nargout > 1,


265  dxpdc = zeros(2*n,2);


266  dxpdc(1:2:end,1) = ones(n,1);


267  dxpdc(2:2:end,2) = ones(n,1);


268  end;


269 


270 


271  return;


272 


273  % Test of the Jacobians:


274 


275  n = 10;


276 


277  X = 10*randn(3,n);


278  om = randn(3,1);


279  T = [10*randn(2,1);40];


280  f = 1000*rand(2,1);


281  c = 1000*randn(2,1);


282  k = 0.5*randn(5,1);


283  alpha = 0.01*randn(1,1);


284 


285  [x,dxdom,dxdT,dxdf,dxdc,dxdk,dxdalpha] = project_points2(X,om,T,f,c,k,alpha);


286 


287 


288  % Test on om: OK


289 


290  dom = 0.000000001 * norm(om)*randn(3,1);


291  om2 = om + dom;


292 


293  [x2] = project_points2(X,om2,T,f,c,k,alpha);


294 


295  x_pred = x + reshape(dxdom * dom,2,n);


296 


297 


298  norm(x2x)/norm(x2  x_pred)


299 


300 


301  % Test on T: OK!!


302 


303  dT = 0.0001 * norm(T)*randn(3,1);


304  T2 = T + dT;


305 


306  [x2] = project_points2(X,om,T2,f,c,k,alpha);


307 


308  x_pred = x + reshape(dxdT * dT,2,n);


309 


310 


311  norm(x2x)/norm(x2  x_pred)


312 


313 


314 


315  % Test on f: OK!!


316 


317  df = 0.001 * norm(f)*randn(2,1);


318  f2 = f + df;


319 


320  [x2] = project_points2(X,om,T,f2,c,k,alpha);


321 


322  x_pred = x + reshape(dxdf * df,2,n);


323 


324 


325  norm(x2x)/norm(x2  x_pred)


326 


327 


328  % Test on c: OK!!


329 


330  dc = 0.01 * norm(c)*randn(2,1);


331  c2 = c + dc;


332 


333  [x2] = project_points2(X,om,T,f,c2,k,alpha);


334 


335  x_pred = x + reshape(dxdc * dc,2,n);


336 


337  norm(x2x)/norm(x2  x_pred)


338 


339  % Test on k: OK!!


340 


341  dk = 0.001 * norm(k)*randn(5,1);


342  k2 = k + dk;


343 


344  [x2] = project_points2(X,om,T,f,c,k2,alpha);


345 


346  x_pred = x + reshape(dxdk * dk,2,n);


347 


348  norm(x2x)/norm(x2  x_pred)


349 


350 


351  % Test on alpha: OK!!


352 


353  dalpha = 0.001 * norm(k)*randn(1,1);


354  alpha2 = alpha + dalpha;


355 


356  [x2] = project_points2(X,om,T,f,c,k,alpha2);


357 


358  x_pred = x + reshape(dxdalpha * dalpha,2,n);


359 


360  norm(x2x)/norm(x2  x_pred)

