[37] | 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 |
---|