[725] | 1 | function [H,Hnorm,inv_Hnorm] = compute_homography(m,M);
|
---|
| 2 |
|
---|
| 3 | %compute_homography
|
---|
| 4 | %
|
---|
| 5 | %[H,Hnorm,inv_Hnorm] = compute_homography(m,M)
|
---|
| 6 | %
|
---|
| 7 | %Computes the planar homography between the point coordinates on the plane (M) and the image
|
---|
| 8 | %point coordinates (m).
|
---|
| 9 | %
|
---|
| 10 | %INPUT: m: homogeneous coordinates in the image plane (3xN matrix)
|
---|
| 11 | % M: homogeneous coordinates in the plane in 3D (3xN matrix)
|
---|
| 12 | %
|
---|
| 13 | %OUTPUT: H: Homography matrix (3x3 homogeneous matrix)
|
---|
| 14 | % Hnorm: Normalization matrix used on the points before homography computation
|
---|
| 15 | % (useful for numerical stability is points in pixel coordinates)
|
---|
| 16 | % inv_Hnorm: The inverse of Hnorm
|
---|
| 17 | %
|
---|
| 18 | %Definition: m ~ H*M where "~" means equal up to a non zero scalar factor.
|
---|
| 19 | %
|
---|
| 20 | %Method: First computes an initial guess for the homography through quasi-linear method.
|
---|
| 21 | % Then, if the total number of points is larger than 4, optimize the solution by minimizing
|
---|
| 22 | % the reprojection error (in the least squares sense).
|
---|
| 23 | %
|
---|
| 24 | %
|
---|
| 25 | %Important functions called within that program:
|
---|
| 26 | %
|
---|
| 27 | %comp_distortion_oulu: Undistorts pixel coordinates.
|
---|
| 28 | %
|
---|
| 29 | %compute_homography.m: Computes the planar homography between points on the grid in 3D, and the image plane.
|
---|
| 30 | %
|
---|
| 31 | %project_points.m: Computes the 2D image projections of a set of 3D points, and also returns te Jacobian
|
---|
| 32 | % matrix (derivative with respect to the intrinsic and extrinsic parameters).
|
---|
| 33 | % This function is called within the minimization loop.
|
---|
| 34 |
|
---|
| 35 |
|
---|
| 36 |
|
---|
| 37 |
|
---|
| 38 | Np = size(m,2);
|
---|
| 39 |
|
---|
| 40 | if size(m,1)<3,
|
---|
| 41 | m = [m;ones(1,Np)];
|
---|
| 42 | end;
|
---|
| 43 |
|
---|
| 44 | if size(M,1)<3,
|
---|
| 45 | M = [M;ones(1,Np)];
|
---|
| 46 | end;
|
---|
| 47 |
|
---|
| 48 |
|
---|
| 49 | m = m ./ (ones(3,1)*m(3,:));
|
---|
| 50 | M = M ./ (ones(3,1)*M(3,:));
|
---|
| 51 |
|
---|
| 52 | % Prenormalization of point coordinates (very important):
|
---|
| 53 | % (Affine normalization)
|
---|
| 54 |
|
---|
| 55 | ax = m(1,:);
|
---|
| 56 | ay = m(2,:);
|
---|
| 57 |
|
---|
| 58 | mxx = mean(ax);
|
---|
| 59 | myy = mean(ay);
|
---|
| 60 | ax = ax - mxx;
|
---|
| 61 | ay = ay - myy;
|
---|
| 62 |
|
---|
| 63 | scxx = mean(abs(ax));
|
---|
| 64 | scyy = mean(abs(ay));
|
---|
| 65 |
|
---|
| 66 |
|
---|
| 67 | Hnorm = [1/scxx 0 -mxx/scxx;0 1/scyy -myy/scyy;0 0 1];
|
---|
| 68 | inv_Hnorm = [scxx 0 mxx ; 0 scyy myy; 0 0 1];
|
---|
| 69 |
|
---|
| 70 | mn = Hnorm*m;
|
---|
| 71 |
|
---|
| 72 | % Compute the homography between m and mn:
|
---|
| 73 |
|
---|
| 74 | % Build the matrix:
|
---|
| 75 |
|
---|
| 76 | L = zeros(2*Np,9);
|
---|
| 77 |
|
---|
| 78 | L(1:2:2*Np,1:3) = M';
|
---|
| 79 | L(2:2:2*Np,4:6) = M';
|
---|
| 80 | L(1:2:2*Np,7:9) = -((ones(3,1)*mn(1,:)).* M)';
|
---|
| 81 | L(2:2:2*Np,7:9) = -((ones(3,1)*mn(2,:)).* M)';
|
---|
| 82 |
|
---|
| 83 | if Np > 4,
|
---|
| 84 | L = L'*L;
|
---|
| 85 | end;
|
---|
| 86 |
|
---|
| 87 | [U,S,V] = svd(L);
|
---|
| 88 |
|
---|
| 89 | hh = V(:,9);
|
---|
| 90 | hh = hh/hh(9);
|
---|
| 91 |
|
---|
| 92 | Hrem = reshape(hh,3,3)';
|
---|
| 93 | %Hrem = Hrem / Hrem(3,3);
|
---|
| 94 |
|
---|
| 95 |
|
---|
| 96 | % Final homography:
|
---|
| 97 |
|
---|
| 98 | H = inv_Hnorm*Hrem;
|
---|
| 99 |
|
---|
| 100 | if 0,
|
---|
| 101 | m2 = H*M;
|
---|
| 102 | m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)];
|
---|
| 103 | merr = m(1:2,:) - m2;
|
---|
| 104 | end;
|
---|
| 105 |
|
---|
| 106 | %keyboard;
|
---|
| 107 |
|
---|
| 108 | %%% Homography refinement if there are more than 4 points:
|
---|
| 109 |
|
---|
| 110 | if Np > 4,
|
---|
| 111 |
|
---|
| 112 | % Final refinement:
|
---|
| 113 | hhv = reshape(H',9,1);
|
---|
| 114 | hhv = hhv(1:8);
|
---|
| 115 |
|
---|
| 116 | for iter=1:10,
|
---|
| 117 |
|
---|
| 118 |
|
---|
| 119 |
|
---|
| 120 | mrep = H * M;
|
---|
| 121 |
|
---|
| 122 | J = zeros(2*Np,8);
|
---|
| 123 |
|
---|
| 124 | MMM = (M ./ (ones(3,1)*mrep(3,:)));
|
---|
| 125 |
|
---|
| 126 | J(1:2:2*Np,1:3) = -MMM';
|
---|
| 127 | J(2:2:2*Np,4:6) = -MMM';
|
---|
| 128 |
|
---|
| 129 | mrep = mrep ./ (ones(3,1)*mrep(3,:));
|
---|
| 130 |
|
---|
| 131 | m_err = m(1:2,:) - mrep(1:2,:);
|
---|
| 132 | m_err = m_err(:);
|
---|
| 133 |
|
---|
| 134 | MMM2 = (ones(3,1)*mrep(1,:)) .* MMM;
|
---|
| 135 | MMM3 = (ones(3,1)*mrep(2,:)) .* MMM;
|
---|
| 136 |
|
---|
| 137 | J(1:2:2*Np,7:8) = MMM2(1:2,:)';
|
---|
| 138 | J(2:2:2*Np,7:8) = MMM3(1:2,:)';
|
---|
| 139 |
|
---|
| 140 | MMM = (M ./ (ones(3,1)*mrep(3,:)))';
|
---|
| 141 |
|
---|
| 142 | hh_innov = inv(J'*J)*J'*m_err;
|
---|
| 143 |
|
---|
| 144 | hhv_up = hhv - hh_innov;
|
---|
| 145 |
|
---|
| 146 | H_up = reshape([hhv_up;1],3,3)';
|
---|
| 147 |
|
---|
| 148 | %norm(m_err)
|
---|
| 149 | %norm(hh_innov)
|
---|
| 150 |
|
---|
| 151 | hhv = hhv_up;
|
---|
| 152 | H = H_up;
|
---|
| 153 |
|
---|
| 154 | end;
|
---|
| 155 |
|
---|
| 156 |
|
---|
| 157 | end;
|
---|
| 158 |
|
---|
| 159 | if 0,
|
---|
| 160 | m2 = H*M;
|
---|
| 161 | m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)];
|
---|
| 162 | merr = m(1:2,:) - m2;
|
---|
| 163 | end;
|
---|
| 164 |
|
---|
| 165 | return;
|
---|
| 166 |
|
---|
| 167 | %test of Jacobian
|
---|
| 168 |
|
---|
| 169 | mrep = H*M;
|
---|
| 170 | mrep = mrep ./ (ones(3,1)*mrep(3,:));
|
---|
| 171 |
|
---|
| 172 | m_err = mrep(1:2,:) - m(1:2,:);
|
---|
| 173 | figure(8);
|
---|
| 174 | plot(m_err(1,:),m_err(2,:),'r+');
|
---|
| 175 | std(m_err')
|
---|