[37] | 1 | % estimateLambda ... estimate some initial projective depth given |
---|
| 2 | % the measurement matrix |
---|
| 3 | % |
---|
| 4 | % This algorithm is based on a paper by P. Sturm and B. Triggs called |
---|
| 5 | % "A Factorization Based Algorithm for Multi-Image Projective Structure |
---|
| 6 | % and motion" (1996) |
---|
| 7 | % Projective depths are estimated using fundamental matrices. |
---|
| 8 | % |
---|
| 9 | % [Lambda] = estimateLambda(Ws) |
---|
| 10 | % |
---|
| 11 | % Ws ...... the 3*nxm measurement matrix (the data should be normalized |
---|
| 12 | % before calling this functions) |
---|
| 13 | % pair .... array of camera pairs containing Fundamental matrices and |
---|
| 14 | % indexes of points used for their computations |
---|
| 15 | % it is the output from RANSAC validation step |
---|
| 16 | % |
---|
| 17 | % Lambda .. nxm matrix containing the estimated projective depths |
---|
| 18 | % |
---|
| 19 | % 09/2001, Dejan Radovic <dradovic@student.ethz.ch> |
---|
| 20 | % 05/2002, Tomas Svoboda <svoboda@vision.ee.ethz.ch> |
---|
| 21 | |
---|
| 22 | % $Author: svoboda $ |
---|
| 23 | % $Revision: 2.0 $ |
---|
| 24 | % $Id: estimateLambda.m,v 2.0 2003/06/19 12:06:49 svoboda Exp $ |
---|
| 25 | % $State: Exp $ |
---|
| 26 | |
---|
| 27 | |
---|
| 28 | function [Lambda] = estimateLambda(Ws,pair) |
---|
| 29 | n = size(Ws,1)/3; % cameras |
---|
| 30 | m = size(Ws,2); % frames |
---|
| 31 | |
---|
| 32 | % estimate (n-1) fundamental matrices F_12, F_23, ..., F_(n-1)n |
---|
| 33 | F = []; % the fundamental matrices |
---|
| 34 | e = []; % the epipoles (as columns) |
---|
| 35 | Lambda = ones(n,m); % the estimated projective depths |
---|
| 36 | for i=1:n-1 |
---|
| 37 | j=i+1; |
---|
| 38 | F_ij = pair(i).F; |
---|
| 39 | % compute epipole from F_ij*e_ij == 0 |
---|
| 40 | [U,S,V] = svd(F_ij,0); |
---|
| 41 | % diag(S)' |
---|
| 42 | e_ij = V(:,size(V,2)); |
---|
| 43 | for p=pair(i).idxin, |
---|
| 44 | q_ip = Ws(i*3-2:i*3,p); |
---|
| 45 | q_jp = Ws(j*3-2:j*3,p); |
---|
| 46 | Lambda(j,p) = Lambda(i,p)*((norm(cross(e_ij,q_ip)))^2/sum(cross(e_ij,q_ip).*(F_ij'*q_jp))); |
---|
| 47 | % Lambda(j,p) = Lambda(i,p)*(sum((F_ij'*q_jp).*cross(e_ij,q_ip))/(norm(F_ij'*q_jp))^2); |
---|
| 48 | end |
---|
| 49 | end |
---|
| 50 | |
---|
| 51 | return |
---|
| 52 | |
---|
| 53 | |
---|
| 54 | |
---|
| 55 | |
---|
| 56 | |
---|
| 57 | |
---|