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

Last change on this file since 940 was 926, checked in by sommeria, 8 years ago

geometry cqlib updated

File size: 3.5 KB
Line 
1function [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
38Np = size(m,2);
39
40if size(m,1)<3,
41   m = [m;ones(1,Np)];
42end;
43
44if size(M,1)<3,
45   M = [M;ones(1,Np)];
46end;
47
48
49m = m ./ (ones(3,1)*m(3,:));
50M = M ./ (ones(3,1)*M(3,:));
51
52% Prenormalization of point coordinates (very important):
53% (Affine normalization)
54
55ax = m(1,:);
56ay = m(2,:);
57
58mxx = mean(ax);
59myy = mean(ay);
60ax = ax - mxx;
61ay = ay - myy;
62
63scxx = mean(abs(ax));
64scyy = mean(abs(ay));
65
66
67Hnorm = [1/scxx 0 -mxx/scxx;0 1/scyy -myy/scyy;0 0 1];
68inv_Hnorm = [scxx 0 mxx ; 0 scyy myy; 0 0 1];
69
70mn = Hnorm*m;
71
72% Compute the homography between m and mn:
73
74% Build the matrix:
75
76L = zeros(2*Np,9);
77
78L(1:2:2*Np,1:3) = M';
79L(2:2:2*Np,4:6) = M';
80L(1:2:2*Np,7:9) = -((ones(3,1)*mn(1,:)).* M)';
81L(2:2:2*Np,7:9) = -((ones(3,1)*mn(2,:)).* M)';
82
83if Np > 4,
84        L = L'*L;
85end;
86
87[U,S,V] = svd(L);
88
89hh = V(:,9);
90hh = hh/hh(9);
91
92Hrem = reshape(hh,3,3)';
93%Hrem = Hrem / Hrem(3,3);
94
95
96% Final homography:
97
98H = inv_Hnorm*Hrem;
99
100if 0,
101   m2 = H*M;
102   m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)];
103   merr = m(1:2,:) - m2;
104end;
105
106%keyboard;
107 
108%%% Homography refinement if there are more than 4 points:
109
110if 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
157end;
158
159if 0,
160   m2 = H*M;
161   m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)];
162   merr = m(1:2,:) - m2;
163end;
164
165return;
166
167%test of Jacobian
168
169mrep = H*M;
170mrep = mrep ./ (ones(3,1)*mrep(3,:));
171
172m_err = mrep(1:2,:) - m(1:2,:);
173figure(8);
174plot(m_err(1,:),m_err(2,:),'r+');
175std(m_err')
Note: See TracBrowser for help on using the repository browser.