[810]  1  %=======================================================================


[908]  2  % Copyright 20082015, LEGI UMR 5519 / CNRS UJF GINP, Grenoble, France


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

