1  function [omckk,Tckk,Rckk,H,x,ex,JJ] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond),


2 


3  %compute_extrinsic


4  %


5  %[omckk,Tckk,Rckk,H,x,ex] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c)


6  %


7  %Computes the extrinsic parameters attached to a 3D structure X_kk given its projection


8  %on the image plane x_kk and the intrinsic camera parameters fc, cc and kc.


9  %Works with planar and nonplanar structures.


10  %


11  %INPUT: x_kk: Feature locations on the images


12  % X_kk: Corresponding grid coordinates


13  % fc: Camera focal length


14  % cc: Principal point coordinates


15  % kc: Distortion coefficients


16  % alpha_c: Skew coefficient


17  %


18  %OUTPUT: omckk: 3D rotation vector attached to the grid positions in space


19  % Tckk: 3D translation vector attached to the grid positions in space


20  % Rckk: 3D rotation matrices corresponding to the omc vectors


21  % H: Homography between points on the grid and points on the image plane (in pixel)


22  % This makes sense only if the planar that is used in planar.


23  % x: Reprojections of the points on the image plane


24  % ex: Reprojection error: ex = x_kk  x;


25  %


26  %Method: Computes the normalized point coordinates, then computes the 3D pose


27  %


28  %Important functions called within that program:


29  %


30  %normalize_pixel: Computes the normalize image point coordinates.


31  %


32  %pose3D: Computes the 3D pose of the structure given the normalized image projection.


33  %


34  %project_points.m: Computes the 2D image projections of a set of 3D points


35 


36 


37 


38  if nargin < 8,


39  thresh_cond = inf;


40  end;


41 


42 


43  if nargin < 7,


44  MaxIter = 20;


45  end;


46 


47 


48  if nargin < 6,


49  alpha_c = 0;


50  if nargin < 5,


51  kc = zeros(5,1);


52  if nargin < 4,


53  cc = zeros(2,1);


54  if nargin < 3,


55  fc = ones(2,1);


56  if nargin < 2,


57  error('Need 2D projections and 3D points (in compute_extrinsic.m)');


58  return;


59  end;


60  end;


61  end;


62  end;


63  end;


64 


65  % Initialization:


66 


67  [omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c);


68 


69  % Refinement:


70  [omckk,Tckk,Rckk,JJ] = compute_extrinsic_refine(omckk,Tckk,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond);


71 


72 


73  % computation of the homography (not useful in the end)


74 


75  H = [Rckk(:,1:2) Tckk];


76 


77  % Computes the reprojection error in pixels:


78 


79  x = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);


80 


81  ex = x_kk  x;


82 


83 


84  % Converts the homography in pixel units:


85 


86  KK = [fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2); 0 0 1];


87 


88  H = KK*H;


89 


90 


91 


92 


93  return;


94 


95 


96  % Test of compte extrinsic:


97 


98  Np = 4;


99  sx = 10;


100  sy = 10;


101  sz = 5;


102 


103  om = randn(3,1);


104  T = [0;0;100];


105 


106  noise = 2/1000;


107 


108  XX = [sx*randn(1,Np);sy*randn(1,Np);sz*randn(1,Np)];


109  xx = project_points(XX,om,T);


110 


111  xxn = xx + noise * randn(2,Np);


112 


113  [omckk,Tckk] = compute_extrinsic(xxn,XX);


114 


115  [om omckk omomckk]


116  [T Tckk TTckk]


117 


118  figure(3);


119  plot(xx(1,:),xx(2,:),'r+');


120  hold on;


121  plot(xxn(1,:),xxn(2,:),'g+');


122  hold off;

