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


2  % Copyright 20082015, 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 [omckk,Tckk,Rckk] = compute_extrinsic_init(x_kk,X_kk,fc,cc,kc,alpha_c),


20 


21  %compute_extrinsic


22  %


23  %[omckk,Tckk,Rckk] = compute_extrinsic_init(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  %


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


41  %


42  %Important functions called within that program:


43  %


44  %normalize_pixel: Computes the normalize image point coordinates.


45  %


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


47  %


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


49 


50 


51 


52  if nargin < 6,


53  alpha_c = 0;


54  if nargin < 5,


55  kc = zeros(5,1);


56  if nargin < 4,


57  cc = zeros(2,1);


58  if nargin < 3,


59  fc = ones(2,1);


60  if nargin < 2,


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


62  return;


63  end;


64  end;


65  end;


66  end;


67  end;


68 


69 


70  %keyboard;


71 


72  % Compute the normalized coordinates:


73 


74  xn = normalize_pixel(x_kk,fc,cc,kc,alpha_c);


75 


76 


77 


78  Np = size(xn,2);


79 


80  %% Check for planarity of the structure:


81  %keyboard;


82 


83  X_mean = mean(X_kk')';


84 


85  Y = X_kk  (X_mean*ones(1,Np));


86 


87  YY = Y*Y';


88 


89  [U,S,V] = svd(YY);


90 


91  r = S(3,3)/S(2,2);


92 


93  %keyboard;


94 


95 


96  if (r < 1e3)(Np < 5), %1e3, %1e4, %norm(X_kk(3,:)) < eps, % Test of planarity


97 


98  %fprintf(1,'Planar structure detected: r=%f\n',r);


99 


100  % Transform the plane to bring it in the Z=0 plane:


101 


102  R_transform = V';


103 


104  %norm(R_transform(1:2,3))


105 


106  if norm(R_transform(1:2,3)) < 1e6,


107  R_transform = eye(3);


108  end;


109 


110  if det(R_transform) < 0, R_transform = R_transform; end;


111 


112  T_transform = (R_transform)*X_mean;


113 


114  X_new = R_transform*X_kk + T_transform*ones(1,Np);


115 


116 


117  % Compute the planar homography:


118 


119  H = compute_homography(xn,X_new(1:2,:));


120 


121  % Deembed the motion parameters from the homography:


122 


123  sc = mean([norm(H(:,1));norm(H(:,2))]);


124 


125  H = H/sc;


126 


127  % Extra normalization for some reasons...


128  %H(:,1) = H(:,1)/norm(H(:,1));


129  %H(:,2) = H(:,2)/norm(H(:,2));


130 


131  if 0, %%% Some tests for myself... the opposite sign solution leads to negative depth!!!


132 


133  % Case#1: no opposite sign:


134 


135  omckk1 = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]);


136  Rckk1 = rodrigues(omckk1);


137  Tckk1 = H(:,3);


138 


139  Hs1 = [Rckk1(:,1:2) Tckk1];


140  xn1 = Hs1*[X_new(1:2,:);ones(1,Np)];


141  xn1 = [xn1(1,:)./xn1(3,:) ; xn1(2,:)./xn1(3,:)];


142  e1 = xn1  xn;


143 


144  % Case#2: opposite sign:


145 


146  omckk2 = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]);


147  Rckk2 = rodrigues(omckk2);


148  Tckk2 = H(:,3);


149 


150  Hs2 = [Rckk2(:,1:2) Tckk2];


151  xn2 = Hs2*[X_new(1:2,:);ones(1,Np)];


152  xn2 = [xn2(1,:)./xn2(3,:) ; xn2(2,:)./xn2(3,:)];


153  e2 = xn2  xn;


154 


155  if 1, %norm(e1) < norm(e2),


156  omckk = omckk1;


157  Tckk = Tckk1;


158  Rckk = Rckk1;


159  else


160  omckk = omckk2;


161  Tckk = Tckk2;


162  Rckk = Rckk2;


163  end;


164 


165  else


166 


167  u1 = H(:,1);


168  u1 = u1 / norm(u1);


169  u2 = H(:,2)  dot(u1,H(:,2)) * u1;


170  u2 = u2 / norm(u2);


171  u3 = cross(u1,u2);


172  RRR = [u1 u2 u3];


173  omckk = rodrigues(RRR);


174 


175  %omckk = rodrigues([H(:,1:2) cross(H(:,1),H(:,2))]);


176  Rckk = rodrigues(omckk);


177  Tckk = H(:,3);


178 


179  end;


180 


181 


182 


183  %If Xc = Rckk * X_new + Tckk, then Xc = Rckk * R_transform * X_kk + Tckk + T_transform


184 


185  Tckk = Tckk + Rckk* T_transform;


186  Rckk = Rckk * R_transform;


187 


188  omckk = rodrigues(Rckk);


189  Rckk = rodrigues(omckk);


190 


191 


192  else


193 


194  %fprintf(1,'Non planar structure detected: r=%f\n',r);


195 


196  % Computes an initial guess for extrinsic parameters (works for general 3d structure, not planar!!!):


197  % The DLT method is applied here!!


198 


199  J = zeros(2*Np,12);


200 


201  xX = (ones(3,1)*xn(1,:)).*X_kk;


202  yX = (ones(3,1)*xn(2,:)).*X_kk;


203 


204  J(1:2:end,[1 4 7]) = X_kk';


205  J(2:2:end,[2 5 8]) = X_kk';


206  J(1:2:end,[3 6 9]) = xX';


207  J(2:2:end,[3 6 9]) = yX';


208  J(1:2:end,12) = xn(1,:)';


209  J(2:2:end,12) = xn(2,:)';


210  J(1:2:end,10) = ones(Np,1);


211  J(2:2:end,11) = ones(Np,1);


212 


213  JJ = J'*J;


214  [U,S,V] = svd(JJ);


215 


216  RR = reshape(V(1:9,12),3,3);


217 


218  if det(RR) < 0,


219  V(:,12) = V(:,12);


220  RR = RR;


221  end;


222 


223  [Ur,Sr,Vr] = svd(RR);


224 


225  Rckk = Ur*Vr';


226 


227  sc = norm(V(1:9,12)) / norm(Rckk(:));


228  Tckk = V(10:12,12)/sc;


229 


230  omckk = rodrigues(Rckk);


231  Rckk = rodrigues(omckk);


232 


233  end;

