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 


19  function [H,Hnorm,inv_Hnorm] = compute_homography(m,M);


20 


21  %compute_homography


22  %


23  %[H,Hnorm,inv_Hnorm] = compute_homography(m,M)


24  %


25  %Computes the planar homography between the point coordinates on the plane (M) and the image


26  %point coordinates (m).


27  %


28  %INPUT: m: homogeneous coordinates in the image plane (3xN matrix)


29  % M: homogeneous coordinates in the plane in 3D (3xN matrix)


30  %


31  %OUTPUT: H: Homography matrix (3x3 homogeneous matrix)


32  % Hnorm: Normalization matrix used on the points before homography computation


33  % (useful for numerical stability is points in pixel coordinates)


34  % inv_Hnorm: The inverse of Hnorm


35  %


36  %Definition: m ~ H*M where "~" means equal up to a non zero scalar factor.


37  %


38  %Method: First computes an initial guess for the homography through quasilinear method.


39  % Then, if the total number of points is larger than 4, optimize the solution by minimizing


40  % the reprojection error (in the least squares sense).


41  %


42  %


43  %Important functions called within that program:


44  %


45  %comp_distortion_oulu: Undistorts pixel coordinates.


46  %


47  %compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane.


48  %


49  %project_points.m: Computes the 2D image projections of a set of 3D points, and also returns te Jacobian


50  % matrix (derivative with respect to the intrinsic and extrinsic parameters).


51  % This function is called within the minimization loop.


52 


53 


54 


55 


56  Np = size(m,2);


57 


58  if size(m,1)<3,


59  m = [m;ones(1,Np)];


60  end;


61 


62  if size(M,1)<3,


63  M = [M;ones(1,Np)];


64  end;


65 


66 


67  m = m ./ (ones(3,1)*m(3,:));


68  M = M ./ (ones(3,1)*M(3,:));


69 


70  % Prenormalization of point coordinates (very important):


71  % (Affine normalization)


72 


73  ax = m(1,:);


74  ay = m(2,:);


75 


76  mxx = mean(ax);


77  myy = mean(ay);


78  ax = ax  mxx;


79  ay = ay  myy;


80 


81  scxx = mean(abs(ax));


82  scyy = mean(abs(ay));


83 


84 


85  Hnorm = [1/scxx 0 mxx/scxx;0 1/scyy myy/scyy;0 0 1];


86  inv_Hnorm = [scxx 0 mxx ; 0 scyy myy; 0 0 1];


87 


88  mn = Hnorm*m;


89 


90  % Compute the homography between m and mn:


91 


92  % Build the matrix:


93 


94  L = zeros(2*Np,9);


95 


96  L(1:2:2*Np,1:3) = M';


97  L(2:2:2*Np,4:6) = M';


98  L(1:2:2*Np,7:9) = ((ones(3,1)*mn(1,:)).* M)';


99  L(2:2:2*Np,7:9) = ((ones(3,1)*mn(2,:)).* M)';


100 


101  if Np > 4,


102  L = L'*L;


103  end;


104 


105  [U,S,V] = svd(L);


106 


107  hh = V(:,9);


108  hh = hh/hh(9);


109 


110  Hrem = reshape(hh,3,3)';


111  %Hrem = Hrem / Hrem(3,3);


112 


113 


114  % Final homography:


115 


116  H = inv_Hnorm*Hrem;


117 


118  if 0,


119  m2 = H*M;


120  m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)];


121  merr = m(1:2,:)  m2;


122  end;


123 


124  %keyboard;


125 


126  %%% Homography refinement if there are more than 4 points:


127 


128  if Np > 4,


129 


130  % Final refinement:


131  hhv = reshape(H',9,1);


132  hhv = hhv(1:8);


133 


134  for iter=1:10,


135 


136 


137 


138  mrep = H * M;


139 


140  J = zeros(2*Np,8);


141 


142  MMM = (M ./ (ones(3,1)*mrep(3,:)));


143 


144  J(1:2:2*Np,1:3) = MMM';


145  J(2:2:2*Np,4:6) = MMM';


146 


147  mrep = mrep ./ (ones(3,1)*mrep(3,:));


148 


149  m_err = m(1:2,:)  mrep(1:2,:);


150  m_err = m_err(:);


151 


152  MMM2 = (ones(3,1)*mrep(1,:)) .* MMM;


153  MMM3 = (ones(3,1)*mrep(2,:)) .* MMM;


154 


155  J(1:2:2*Np,7:8) = MMM2(1:2,:)';


156  J(2:2:2*Np,7:8) = MMM3(1:2,:)';


157 


158  MMM = (M ./ (ones(3,1)*mrep(3,:)))';


159 


160  hh_innov = inv(J'*J)*J'*m_err;


161 


162  hhv_up = hhv  hh_innov;


163 


164  H_up = reshape([hhv_up;1],3,3)';


165 


166  %norm(m_err)


167  %norm(hh_innov)


168 


169  hhv = hhv_up;


170  H = H_up;


171 


172  end;


173 


174 


175  end;


176 


177  if 0,


178  m2 = H*M;


179  m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)];


180  merr = m(1:2,:)  m2;


181  end;


182 


183  return;


184 


185  %test of Jacobian


186 


187  mrep = H*M;


188  mrep = mrep ./ (ones(3,1)*mrep(3,:));


189 


190  m_err = mrep(1:2,:)  m(1:2,:);


191  figure(8);


192  plot(m_err(1,:),m_err(2,:),'r+');


193  std(m_err')

