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 | |
---|