1 | %fill_mm Proj. reconstruction from MM. |
---|
2 | % |
---|
3 | % [P,X, u1,u2, info] = fill_mm(M, opt) |
---|
4 | % |
---|
5 | % The concept of the central image strength and of the sequence is used. |
---|
6 | % |
---|
7 | % Parameters: |
---|
8 | % M .. measurement matrix (MM) with homogeneous image points with NaNs |
---|
9 | % standing for the unknown elements in all three coordinates |
---|
10 | % size(M) = [3m, n] |
---|
11 | % opt .. options with default values in (): |
---|
12 | % .strategy(-1) .. -1 choose the best of the following: |
---|
13 | % 0 sequence |
---|
14 | % -2 all strategies with the central image |
---|
15 | % 1..m the central image of the corresponding No. |
---|
16 | % .create_nullspace.trial_coef(1) |
---|
17 | % .create_nullspace.threshold(.01) |
---|
18 | % .tol(1e-6) .. tolerance on rows of well computed basis of MM, |
---|
19 | % see approximate |
---|
20 | % .no_factorization(0) .. if 1, no factorization performed |
---|
21 | % .metric(1) .. the metric used for reproj. error |
---|
22 | % 1 .. square root of sum of squares of x- and y-coordinates |
---|
23 | % 2 .. std of x- and y-coordinates |
---|
24 | % .verbose(1) .. whether display info |
---|
25 | % |
---|
26 | % Return parameters: |
---|
27 | % P, X .. estimated projective structure (X) and motion (P) |
---|
28 | % u1, u2 .. unrecovered images (u1) and points (u2) |
---|
29 | % size(P) = [3*(m-length(u1)),4], size(X) = [4,n-length(u2)] |
---|
30 | % info.omega .. info, e.g. predictor functions, about the used strategy |
---|
31 | |
---|
32 | function [P,X, u1,u2, info] = fill_mm(M, opt) |
---|
33 | |
---|
34 | if ~isfield(opt, 'strategy'), opt.strategy = -1; end |
---|
35 | if ~isfield(opt, 'create_nullspace') | ... |
---|
36 | ~isfield(opt.create_nullspace, 'trial_coef') |
---|
37 | opt.create_nullspace.trial_coef = 1; end % 10 |
---|
38 | if ~isfield(opt.create_nullspace, 'threshold') |
---|
39 | opt.create_nullspace.threshold = .01; end |
---|
40 | if ~isfield(opt, 'tol'), opt.tol = 1e-6; end |
---|
41 | if ~isfield(opt, 'no_factorization'), |
---|
42 | opt.no_factorization = 0; end |
---|
43 | if ~isfield(opt, 'metric'), |
---|
44 | opt.metric = 1; end |
---|
45 | if ~isfield(opt, 'verbose'), |
---|
46 | opt.verbose = 1; end |
---|
47 | if ~isfield(opt.create_nullspace, 'verbose') |
---|
48 | opt.create_nullspace.verbose = opt.verbose; end |
---|
49 | |
---|
50 | if ~opt.verbose, fprintf('Repr. error in proj. space (no fact.'); |
---|
51 | if ~opt.no_factorization, fprintf('/fact.'); end |
---|
52 | if ~opt.no_BA, fprintf('/BA'); end; fprintf(') is ... '); end |
---|
53 | |
---|
54 | nbeg = size(M,2); |
---|
55 | ptbeg = find(sum(~isnan(M(1:3:end,:))) >= 2); u2beg = setdiff(1:nbeg,ptbeg); |
---|
56 | M = M(:,ptbeg); |
---|
57 | if ~isempty(u2beg), |
---|
58 | fprintf('Removed correspondences in < 2 images (%d):%s\n', length(u2beg),... |
---|
59 | sprintf(' %d', u2beg(1:min(20, length(u2beg))))); end |
---|
60 | |
---|
61 | [m n] = size(M); m = m/3; M0 = M; |
---|
62 | cols = []; recoverable = inf; Omega = []; |
---|
63 | |
---|
64 | % names of strategies |
---|
65 | switch opt.strategy |
---|
66 | case -1 % use all strategies |
---|
67 | Omega{end+1}.name = 'seq'; |
---|
68 | for i = 1:m, Omega{end+1}.name = 'cent'; Omega{end}.ind = i; end |
---|
69 | case 0 % sequence |
---|
70 | Omega{end+1}.name = 'seq'; |
---|
71 | case -2 % use all strategies with the central image |
---|
72 | for i = 1:m, Omega{end+1}.name = 'cent'; Omega{end}.ind = i; end |
---|
73 | otherwise % the central image of corresponding No. |
---|
74 | Omega{end+1}.name = 'cent'; Omega{end}.ind = opt.strategy; |
---|
75 | end |
---|
76 | |
---|
77 | added = 1; I = ~isnan(M(1:3:end,:)); info = []; iter = 0; |
---|
78 | while recoverable > 0 ... % not all parts of JIM are restored |
---|
79 | & added % something was added during the last trial |
---|
80 | if opt.verbose, |
---|
81 | disp(sprintf('%d (from %d) recovered columns...', length(cols), n)); end |
---|
82 | |
---|
83 | added = 0; iter = iter + 1; if iter > 10, keyboard; end |
---|
84 | |
---|
85 | [S, F, strengths] = compute_predictions(Omega, I); % for all stragegies |
---|
86 | |
---|
87 | % try the best strategy(s) |
---|
88 | while (~added & max(F) > 0) | ... |
---|
89 | sum(F==0) == length(F) % when no data missing, only rescaling needed |
---|
90 | |
---|
91 | % choose the actually best strategy (sg) |
---|
92 | Omega_F = find(F==max(F)); if length(Omega_F) == 1, sg = Omega_F; else |
---|
93 | if opt.verbose, disp(sprintf('Omega_F:%s.', sprintf(' %d',Omega_F))); end |
---|
94 | ns = S(Omega_F); |
---|
95 | Omega_S = Omega_F(find(ns==max(ns))); if length(Omega_S) >1 & opt.verbose |
---|
96 | disp(sprintf('Omega_S:%s.', sprintf(' %d', Omega_S))); |
---|
97 | disp(['!!! Maybe some other criteria is needed to choose' ... |
---|
98 | ' the better candidate (now the 1st is taken)! !!!']); end |
---|
99 | sg = Omega_S(1); % an arbitrary of Omega_S |
---|
100 | end |
---|
101 | |
---|
102 | [rows, cols, central, info] = set_rows_cols(Omega, sg, F, S, strengths, ... |
---|
103 | I, info, opt); |
---|
104 | F(sg) = -1; % "don't try this strategy anymore" |
---|
105 | if opt.verbose, disp(sprintf('Used images:%s.', sprintf(' %d', rows))); end |
---|
106 | [Mn,T] = normM(M); |
---|
107 | [Pn,X, lambda, u1,u2, info] = fill_mm_sub(Mn(k2i(rows),:), ... |
---|
108 | Mn(k2i(rows),cols), ... |
---|
109 | find(central==rows),opt,info); |
---|
110 | if length(u1)==length(rows) & length(u2)==length(cols) |
---|
111 | if opt.verbose, disp('Nothing recovered.'); end |
---|
112 | else |
---|
113 | if opt.verbose |
---|
114 | if ~isempty(u1), disp(sprintf('u1 = %s', num2str(u1))); end |
---|
115 | if ~isempty(u2) & ~isempty(Pn),disp(sprintf('u2 = %s',num2str(u2)));end |
---|
116 | end |
---|
117 | |
---|
118 | % say (un)recovered in indices of whole M not only M(k2i(rows),cols) |
---|
119 | r1 = rows(setdiff(1:length(rows),u1)); u1 = setdiff(1:m, r1); |
---|
120 | r2 = cols(setdiff(1:length(cols),u2)); u2 = setdiff(1:n, r2); |
---|
121 | P = normMback(Pn, T(r1,:,:)); R = P*X; |
---|
122 | |
---|
123 | % fill holes: use the original data when known as much as possible rather |
---|
124 | % then the new data |
---|
125 | if isempty(r1) | isempty(r2), fill = []; else, |
---|
126 | fill = find(isnan(M(3*r1, r2)) ... % some depths (lambda) could not |
---|
127 | ... % have been computed L2depths |
---|
128 | | (~isnan(M(3*r1,r2)) & isnan(lambda))); end |
---|
129 | M_ = M(k2i(r1),r2); M_(k2i(fill)) = R(k2i(fill)); M(k2i(r1),r2) = M_; |
---|
130 | |
---|
131 | added = length(fill); I = ~isnan(M(1:3:end,:)); |
---|
132 | recoverable = sum(sum(I(:,find(sum(I) >= 2))==0)); |
---|
133 | if opt.verbose, |
---|
134 | disp(sprintf('%d points added, %d recoverable points remain.', ... |
---|
135 | added, recoverable)); end |
---|
136 | |
---|
137 | info.err.no_fact = dist(M0(k2i(r1),r2), R, opt.metric); |
---|
138 | if opt.verbose, |
---|
139 | fprintf('Error (no factorization): %f\n', info.err.no_fact); |
---|
140 | else, fprintf(' %f', info.err.no_fact); end |
---|
141 | |
---|
142 | end |
---|
143 | end |
---|
144 | |
---|
145 | if ~added & sum(~I(:)) > 0 % impossible to recover anything |
---|
146 | P=[];X=[]; u1=1:m; u2=1:nbeg; info.opt = opt; return; end |
---|
147 | end |
---|
148 | |
---|
149 | if length(r1)<2, % rank cannot be 4 |
---|
150 | P=[];X=[]; u1=1:m; u2=1:nbeg; info.opt = opt; return; end |
---|
151 | |
---|
152 | if ~opt.no_factorization |
---|
153 | if opt.verbose |
---|
154 | fprintf(1,'Factorization into structure and motion...'); tic; end |
---|
155 | |
---|
156 | % depths estimated from info.Mdepths (which is approximated by R) |
---|
157 | Mdepths_un = normMback(info.Mdepths, T(r1,:,:)); |
---|
158 | lambda(length(r1), length(r2)) = NaN; |
---|
159 | for i = find(~isnan(M0(3*r1, r2)))', |
---|
160 | lambda(i) = M0(k2i(i)) \ Mdepths_un(k2i(i)); end |
---|
161 | for i = 1:length(r1), B(k2i(i),:) = M(k2i(i),:).*([1;1;1]*lambda(i,:)); end |
---|
162 | fill = find(isnan(M0(3*r1, r2))); B(k2i(fill)) = R(k2i(fill)); |
---|
163 | [Bn,T] = normM(B); |
---|
164 | opt.info_separately = 0; Bn = balance_triplets(Bn, opt); |
---|
165 | if opt.verbose, fprintf(1,'(running svd...'); tic; end |
---|
166 | [u,s,v] = svd(Bn); s1 = sqrt(s(1:4,1:4)); P = u(:,1:4)*s1; X = s1*v(:,1:4)'; |
---|
167 | if opt.verbose, disp([num2str(toc) ' sec)']); end |
---|
168 | P = normMback(P, T); |
---|
169 | |
---|
170 | info.err.fact = dist(M0(k2i(r1),r2), P*X, opt.metric); |
---|
171 | if opt.verbose, fprintf('Error (after factorization): %f\n', info.err.fact); |
---|
172 | else, fprintf(' %f', info.err.fact); end |
---|
173 | end |
---|
174 | |
---|
175 | u2 = union(u2beg, ptbeg(u2)); |
---|
176 | info.opt = opt; |
---|
177 | |
---|
178 | function [S, F, strengths] = compute_predictions(Omega, I) |
---|
179 | % compute predictor functions for all strategies |
---|
180 | |
---|
181 | for sg = 1:length(Omega) |
---|
182 | switch Omega{sg}.name |
---|
183 | case 'seq' |
---|
184 | [b, len] = subseq_longest(I); |
---|
185 | Isub = I; % for now, epipolar geometry is supposed to be computable |
---|
186 | % through the whole sequence. See also lower. |
---|
187 | F(sg) = sum(Isub(:) == 0); |
---|
188 | S(sg) = sum(len); |
---|
189 | case 'cent' |
---|
190 | i = Omega{sg}.ind; |
---|
191 | strengths{i} = strength(i, I, 1); |
---|
192 | F(sg) = strengths{i}.strength(1); % or 2 |
---|
193 | S(sg) = strengths{i}.num_scaled; |
---|
194 | end |
---|
195 | end |
---|
196 | |
---|
197 | |
---|
198 | function result = strength(central, I, general) |
---|
199 | |
---|
200 | %strength Compute the central image strength of an image. |
---|
201 | % |
---|
202 | % result = strength(central, I, general) |
---|
203 | % |
---|
204 | % Parameters: |
---|
205 | % `general' is set to logical true when generalized Jacobs' algorithm for |
---|
206 | % unknown projective depths is supposed. |
---|
207 | % |
---|
208 | % `result' is a stucture containing following fields |
---|
209 | % `strength' is computed strength |
---|
210 | % `good_rows' are the useful rows for the used submatrix |
---|
211 | % `good_cols' "" columns "" |
---|
212 | % `Isub' is the "good" submatrix where ones are known points |
---|
213 | % `num_scaled' is the number of scaled points |
---|
214 | |
---|
215 | if nargin<3, general=0; end |
---|
216 | |
---|
217 | |
---|
218 | [m n] = size(I); |
---|
219 | good_rows = []; |
---|
220 | |
---|
221 | %display(central); |
---|
222 | % fundamental matrices |
---|
223 | for i = setdiff(1:m, central) |
---|
224 | common = find(I(i,:) & I(central,:)); |
---|
225 | if length(common) >= 8 % 7 for 7-points algorithm |
---|
226 | good_rows = [good_rows i]; |
---|
227 | |
---|
228 | % only common points |
---|
229 | if ~general |
---|
230 | I(i,setdiff(1:n,common)) = zeros(1,n-length(common)); end |
---|
231 | else |
---|
232 | I(i,:) = zeros(1,n); |
---|
233 | end |
---|
234 | end |
---|
235 | |
---|
236 | good_rows = union(good_rows,central); |
---|
237 | |
---|
238 | % Jacobs' algorithm needs at least 2 points in each column |
---|
239 | % and its generalized form for unknown depths as well |
---|
240 | present = sum(I); |
---|
241 | good_cols = find(present >= 2); |
---|
242 | |
---|
243 | Isub = I(good_rows,good_cols); |
---|
244 | |
---|
245 | result.strength = [ sum(Isub(:)==0) size(Isub,1)*size(Isub,2) ]; |
---|
246 | |
---|
247 | % find scaled image points (those in the columns known in the |
---|
248 | % central image) |
---|
249 | scaled_cols = find(Isub(find(central==good_rows),:)==1); |
---|
250 | num_scaled = 0; |
---|
251 | for i=1:length(good_rows), |
---|
252 | scaled = intersect(find(Isub(i,:)==1), scaled_cols); |
---|
253 | num_scaled = num_scaled + length(scaled); |
---|
254 | end |
---|
255 | |
---|
256 | result.good_rows = good_rows; |
---|
257 | result.good_cols = good_cols; |
---|
258 | result.Isub = Isub; |
---|
259 | result.num_scaled = num_scaled; |
---|
260 | |
---|
261 | |
---|
262 | function [rows, cols, central, info] = set_rows_cols(Omega, sg, F, S, ... |
---|
263 | strengths, I,info, opt) |
---|
264 | |
---|
265 | [m n] = size(I); |
---|
266 | switch Omega{sg}.name |
---|
267 | case 'seq' |
---|
268 | rows = 1:m; % for now, epipolar geometry is supposed to be computable |
---|
269 | % through the whole sequence. See also above. |
---|
270 | cols = 1:n; % "" |
---|
271 | Isub = I(rows, cols); |
---|
272 | central = 0; |
---|
273 | case 'cent' |
---|
274 | central = Omega{sg}.ind; |
---|
275 | rows = strengths{central}.good_rows; |
---|
276 | cols = strengths{central}.good_cols; |
---|
277 | Isub = strengths{central}.Isub; |
---|
278 | end |
---|
279 | |
---|
280 | info.omega = Omega{sg}; info.omega.F = F(sg); info.omega.S = S(sg); |
---|
281 | sequence = set_sequence(central, S(sg), Isub, I, opt); |
---|
282 | if ~isfield(info,'sequence'), info.sequence{1} = sequence; |
---|
283 | else, info.sequence{end+1} = sequence; end |
---|
284 | |
---|
285 | |
---|
286 | function sequence = set_sequence(central, num_scaled, Isub, I, opt) |
---|
287 | |
---|
288 | sequence.central = central; |
---|
289 | if isempty(Isub) |
---|
290 | sequence.scaled = 0; |
---|
291 | sequence.missing = 0; |
---|
292 | sequence.used_pts = 0; |
---|
293 | else |
---|
294 | sequence.scaled = num_scaled / sum(Isub(:)) * 100; |
---|
295 | sz = size(Isub); |
---|
296 | sequence.missing = sum(Isub(:) == 0) / (sz(1)*sz(2)) * 100; |
---|
297 | sequence.used_pts = sum(Isub(:)) / sum(I(:)) * 100; |
---|
298 | end |
---|
299 | if opt.verbose |
---|
300 | disp(sprintf(['Image %d is the central image, image points: %.2f %%' ... |
---|
301 | ' scaled, %.2f %% missing, %.2f %% used.'], central, ... |
---|
302 | sequence.scaled, sequence.missing, ... |
---|
303 | sequence.used_pts)); end |
---|
304 | |
---|
305 | %normM Normalize the image coordinates by applying transformations normu. |
---|
306 | function [Mr, T] = normM(M) |
---|
307 | |
---|
308 | for k = 1:size(M,1)/3 |
---|
309 | Tk = normu(M(k2i(k),find(~isnan(M(3*k,:))))); |
---|
310 | Mr(k2i(k),:) = Tk * M(k2i(k),:); T(k,1:3,1:3) = Tk; |
---|
311 | end |
---|
312 | |
---|
313 | %normMback Adapt projective motion, to account for the normalization norm. |
---|
314 | function P = normMback(P, T) |
---|
315 | |
---|
316 | for k = 1:size(P,1)/3 |
---|
317 | Tk = reshape(T(k,1:3,1:3),3,3); |
---|
318 | P(3*k-2:3*k,:) = inv(Tk)*P(3*k-2:3*k,:); |
---|
319 | end |
---|