1 | %=======================================================================
|
---|
2 | % Copyright 2008-2015, LEGI UMR 5519 / CNRS UJF G-INP, Grenoble, France
|
---|
3 | % http://www.legi.grenoble-inp.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 quasi-linear 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')
|
---|