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,JJ] = compute_extrinsic_refine(omc_init,Tc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter,thresh_cond),


20 


21  %compute_extrinsic


22  %


23  %[omckk,Tckk,Rckk] = compute_extrinsic_refine(omc_init,x_kk,X_kk,fc,cc,kc,alpha_c,MaxIter)


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  % MaxIter: Maximum number of iterations


36  %


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


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


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


40 


41  %


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


43  %


44  %Important functions called within that program:


45  %


46  %normalize_pixel: Computes the normalize image point coordinates.


47  %


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


49  %


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


51 


52 


53  if nargin < 10,


54  thresh_cond = inf;


55  end;


56 


57 


58  if nargin < 9,


59  MaxIter = 20;


60  end;


61 


62  if nargin < 8,


63  alpha_c = 0;


64  if nargin < 7,


65  kc = zeros(5,1);


66  if nargin < 6,


67  cc = zeros(2,1);


68  if nargin < 5,


69  fc = ones(2,1);


70  if nargin < 4,


71  error('Need 2D projections and 3D points (in compute_extrinsic_refine.m)');


72  return;


73  end;


74  end;


75  end;


76  end;


77  end;


78 


79 


80  % Initialization:


81 


82  omckk = omc_init;


83  Tckk = Tc_init;


84 


85 


86  % Final optimization (minimize the reprojection error in pixel):


87  % through Gradient Descent:


88 


89  param = [omckk;Tckk];


90 


91  change = 1;


92 


93  iter = 0;


94 


95  %keyboard;


96 


97  %fprintf(1,'Gradient descent iterations: ');


98 


99  while (change > 1e10)&(iter < MaxIter),


100 


101  %fprintf(1,'%d...',iter+1);


102 


103  [x,dxdom,dxdT] = project_points2(X_kk,omckk,Tckk,fc,cc,kc,alpha_c);


104 


105  ex = x_kk  x;


106 


107  %keyboard;


108 


109  JJ = [dxdom dxdT];


110 


111  if cond(JJ) > thresh_cond,


112  change = 0;


113  else


114 


115  JJ2 = JJ'*JJ;


116 


117  param_innov = inv(JJ2)*(JJ')*ex(:);


118  param_up = param + param_innov;


119  change = norm(param_innov)/norm(param_up);


120  param = param_up;


121  iter = iter + 1;


122 


123  omckk = param(1:3);


124  Tckk = param(4:6);


125 


126  end;


127 


128  end;


129 


130  %fprintf(1,'\n');


131 


132  Rckk = rodrigues(omckk);

