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