[810]  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 


[725]  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);

