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

Last change on this file since 1064 was 1000, checked in by g7moreau, 7 years ago
  • Remove tab
File size: 3.6 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
36Np = size(m,2);
37
38if size(m,1)<3,
39    m = [m;ones(1,Np)];
40end;
41
42if size(M,1)<3,
43    M = [M;ones(1,Np)];
44end;
45
46
47m = m ./ (ones(3,1)*m(3,:));
48M = M ./ (ones(3,1)*M(3,:));
49
50% Prenormalization of point coordinates (very important):
51% (Affine normalization)
52
53ax = m(1,:);
54ay = m(2,:);
55
56mxx = mean(ax);
57myy = mean(ay);
58ax = ax - mxx;
59ay = ay - myy;
60
61scxx = mean(abs(ax));
62scyy = mean(abs(ay));
63
64
65Hnorm = [1/scxx 0 -mxx/scxx;0 1/scyy -myy/scyy;0 0 1];
66inv_Hnorm = [scxx 0 mxx ; 0 scyy myy; 0 0 1];
67
68mn = Hnorm*m;
69
70% Compute the homography between m and mn:
71
72% Build the matrix:
73
74L = zeros(2*Np,9);
75
76L(1:2:2*Np,1:3) = M';
77L(2:2:2*Np,4:6) = M';
78L(1:2:2*Np,7:9) = -((ones(3,1)*mn(1,:)).* M)';
79L(2:2:2*Np,7:9) = -((ones(3,1)*mn(2,:)).* M)';
80
81if Np > 4,
82    L = L'*L;
83end;
84
85[U,S,V] = svd(L);
86
87hh = V(:,9);
88hh = hh/hh(9);
89
90Hrem = reshape(hh,3,3)';
91%Hrem = Hrem / Hrem(3,3);
92
93
94% Final homography:
95
96H = inv_Hnorm*Hrem;
97
98if 0,
99    m2 = H*M;
100    m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)];
101    merr = m(1:2,:) - m2;
102end;
103
104%keyboard;
105
106%%% Homography refinement if there are more than 4 points:
107
108if Np > 4,
109
110    % Final refinement:
111    hhv = reshape(H',9,1);
112    hhv = hhv(1:8);
113
114    for iter=1:10,
115
116         mrep = H * M;
117
118         J = zeros(2*Np,8);
119
120         MMM = (M ./ (ones(3,1)*mrep(3,:)));
121
122         J(1:2:2*Np,1:3) = -MMM';
123         J(2:2:2*Np,4:6) = -MMM';
124
125         mrep = mrep ./ (ones(3,1)*mrep(3,:));
126
127         m_err = m(1:2,:) - mrep(1:2,:);
128         m_err = m_err(:);
129
130         MMM2 = (ones(3,1)*mrep(1,:)) .* MMM;
131         MMM3 = (ones(3,1)*mrep(2,:)) .* MMM;
132
133         J(1:2:2*Np,7:8) = MMM2(1:2,:)';
134         J(2:2:2*Np,7:8) = MMM3(1:2,:)';
135
136         MMM = (M ./ (ones(3,1)*mrep(3,:)))';
137
138         hh_innov  = inv(J'*J)*J'*m_err;
139
140         hhv_up = hhv - hh_innov;
141
142         H_up = reshape([hhv_up;1],3,3)';
143
144         %norm(m_err)
145         %norm(hh_innov)
146
147         hhv = hhv_up;
148       H = H_up;
149
150    end;
151end;
152
153if 0,
154    m2 = H*M;
155    m2 = [m2(1,:)./m2(3,:) ; m2(2,:)./m2(3,:)];
156    merr = m(1:2,:) - m2;
157end;
158
159return;
160
161%test of Jacobian
162
163mrep = H*M;
164mrep = mrep ./ (ones(3,1)*mrep(3,:));
165
166m_err = mrep(1:2,:) - m(1:2,:);
167figure(8);
168plot(m_err(1,:),m_err(2,:),'r+');
169std(m_err')
Note: See TracBrowser for help on using the repository browser.