source:trunk/src/toolbox_calib/compute_homography.m@810

Last change on this file since 810 was 810, checked in by g7moreau, 10 years ago
File size: 4.5 KB
Line
1%=======================================================================
2% Copyright 2008-2014, 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
17%=======================================================================
18
19function [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
56Np = size(m,2);
57
58if size(m,1)<3,
59   m = [m;ones(1,Np)];
60end;
61
62if size(M,1)<3,
63   M = [M;ones(1,Np)];
64end;
65
66
67m = m ./ (ones(3,1)*m(3,:));
68M = M ./ (ones(3,1)*M(3,:));
69
70% Prenormalization of point coordinates (very important):
71% (Affine normalization)
72
73ax = m(1,:);
74ay = m(2,:);
75
76mxx = mean(ax);
77myy = mean(ay);
78ax = ax - mxx;
79ay = ay - myy;
80
81scxx = mean(abs(ax));
82scyy = mean(abs(ay));
83
84
85Hnorm = [1/scxx 0 -mxx/scxx;0 1/scyy -myy/scyy;0 0 1];
86inv_Hnorm = [scxx 0 mxx ; 0 scyy myy; 0 0 1];
87
88mn = Hnorm*m;
89
90% Compute the homography between m and mn:
91
92% Build the matrix:
93
94L = zeros(2*Np,9);
95
96L(1:2:2*Np,1:3) = M';
97L(2:2:2*Np,4:6) = M';
98L(1:2:2*Np,7:9) = -((ones(3,1)*mn(1,:)).* M)';
99L(2:2:2*Np,7:9) = -((ones(3,1)*mn(2,:)).* M)';
100
101if Np > 4,
102        L = L'*L;
103end;
104
105[U,S,V] = svd(L);
106
107hh = V(:,9);
108hh = hh/hh(9);
109
110Hrem = reshape(hh,3,3)';
111%Hrem = Hrem / Hrem(3,3);
112
113
114% Final homography:
115
116H = inv_Hnorm*Hrem;
117
118if 0,
119   m2 = H*M;
120   m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)];
121   merr = m(1:2,:) - m2;
122end;
123
124%keyboard;
125
126%%% Homography refinement if there are more than 4 points:
127
128if 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
175end;
176
177if 0,
178   m2 = H*M;
179   m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)];
180   merr = m(1:2,:) - m2;
181end;
182
183return;
184
185%test of Jacobian
186
187mrep = H*M;
188mrep = mrep ./ (ones(3,1)*mrep(3,:));
189
190m_err = mrep(1:2,:) - m(1:2,:);
191figure(8);
192plot(m_err(1,:),m_err(2,:),'r+');
193std(m_err')
Note: See TracBrowser for help on using the repository browser.