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


2  % Copyright 20082016, LEGI UMR 5519 / CNRS UGA 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 [omckk,Tckk,Rckk,H,x,ex,JJ] = compute_extrinsic(x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond),


20 


21  %compute_extrinsic


22  %


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


24  %


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


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


27  %Works with planar and nonplanar structures.


28  %


29  %INPUT: x_kk: Feature locations on the images


30  % X_kk: Corresponding grid coordinates


31  % fc: Camera focal length


32  % cc: Principal point coordinates


33  % kc: Distortion coefficients


34  % alpha_c: Skew coefficient


35  %


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


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


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


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


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


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


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


43  %


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


45  %


46  %Important functions called within that program:


47  %


48  %normalize_pixel: Computes the normalize image point coordinates.


49  %


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


51  %


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


53 


54 


55 


56  if nargin < 8,


57  thresh_cond = inf;


58  end;


59 


60 


61  if nargin < 7,


62  MaxIter = 20;


63  end;


64 


65 


66  if nargin < 6,


67  alpha_c = 0;


68  if nargin < 5,


69  kc = zeros(5,1);


70  if nargin < 4,


71  cc = zeros(2,1);


72  if nargin < 3,


73  fc = ones(2,1);


74  if nargin < 2,


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


76  return;


77  end;


78  end;


79  end;


80  end;


81  end;


82 


83  % Initialization:


84 


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


86 


87  % Refinement:


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


89 


90 


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


92 


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


94 


95  % Computes the reprojection error in pixels:


96 


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


98 


99  ex = x_kk  x;


100 


101 


102  % Converts the homography in pixel units:


103 


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


105 


106  H = KK*H;


107 


108 


109 


110 


111  return;


112 


113 


114  % Test of compte extrinsic:


115 


116  Np = 4;


117  sx = 10;


118  sy = 10;


119  sz = 5;


120 


121  om = randn(3,1);


122  T = [0;0;100];


123 


124  noise = 2/1000;


125 


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


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


128 


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


130 


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


132 


133  [om omckk omomckk]


134  [T Tckk TTckk]


135 


136  figure(3);


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


138  hold on;


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


140  hold off;

