1 | function [Xe,Pe,C,R,T,foc] = selfcalib(Ws,IdMat) |
---|
2 | |
---|
3 | % selfcalib .... performs the self-calibration algorithm on |
---|
4 | % a measurement matrix |
---|
5 | % |
---|
6 | % [Xe,Pe,C,R,T,foc] = selfcalib(Ws) |
---|
7 | % |
---|
8 | % Ws ........ the 3*nxm measurement matrix |
---|
9 | % |
---|
10 | % Xe ........ 4xm matrix containg reconstructed calibration points |
---|
11 | % Pe ........ 3*CAMSx4 matrix containing estimated camera matrices |
---|
12 | % C ......... 4xn matrix containg the camera centers |
---|
13 | % R ......... 3*CAMSx3 matrix containing estimated camera rotation matrices |
---|
14 | % T ......... 3*n matrix containing the camera translation vectors |
---|
15 | % foc ....... CAMSx1 vector containing the focal lengths of the cameras |
---|
16 | |
---|
17 | |
---|
18 | POINTS = size(Ws,2); |
---|
19 | CAMS = size(Ws,1)/3; |
---|
20 | |
---|
21 | if 1 |
---|
22 | % normalize image data |
---|
23 | % (see Hartley, p.91) |
---|
24 | T = []; % the CAMS*3x3 normalization transformations |
---|
25 | for i=1:CAMS |
---|
26 | [X_i, T_i] = isptnorm(Ws(i*3-2:i*3-1,IdMat(i,:)>0)'); |
---|
27 | Ws(i*3-2:i*3-1,IdMat(i,:)>0) = [X_i'; ones(1,sum(IdMat(i,:)>0))]; |
---|
28 | T = [T; T_i]; |
---|
29 | end |
---|
30 | else |
---|
31 | T = repmat(eye(3),CAMS,1); |
---|
32 | end |
---|
33 | |
---|
34 | % estimate projective depths |
---|
35 | Lambda_est = estimateLambda(Ws,IdMat); |
---|
36 | % Lambda_est = ones(CAMS,POINTS); |
---|
37 | |
---|
38 | if 1 |
---|
39 | % normalize estimated lambdas. |
---|
40 | % it is more balancing than normalization |
---|
41 | % Check it again. Probably not correct? |
---|
42 | lambnfr = sum(Lambda_est.^2); |
---|
43 | Lambda_est = sqrt(CAMS)*Lambda_est./repmat(sqrt(lambnfr),CAMS,1); |
---|
44 | lambnfc = sum(Lambda_est'.^2); |
---|
45 | Lambda_est = sqrt(POINTS)*Lambda_est./repmat(sqrt(lambnfc'),1,POINTS); |
---|
46 | end |
---|
47 | |
---|
48 | % no need for negative lambdas |
---|
49 | Lambda_est = abs(Lambda_est); |
---|
50 | |
---|
51 | % Lambda check |
---|
52 | % employing lambdas, the Ws should have rank 4 ! |
---|
53 | if 0 |
---|
54 | lambdaMat = []; |
---|
55 | for i=1:CAMS |
---|
56 | lambdaMat = [lambdaMat; repmat(Lambda_est(i,:),3,1)]; |
---|
57 | end |
---|
58 | Ws_rankcheck = lambdaMat.*Ws; |
---|
59 | [svd(Ws_rankcheck),svd(Ws)] |
---|
60 | end |
---|
61 | |
---|
62 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% |
---|
63 | % compute projective shape and motion |
---|
64 | [P,X,Lambda] = estimatePX(Ws,Lambda_est); |
---|
65 | |
---|
66 | % undo normalization |
---|
67 | for i=1:CAMS |
---|
68 | P(3*i-2:3*i,:) = inv(T(3*i-2:3*i,:))*P(3*i-2:3*i,:); |
---|
69 | end |
---|
70 | |
---|
71 | % Euclidian reconstruction |
---|
72 | warn = 0; |
---|
73 | [Pe,Xe,C,R,T,foc,warn] = euclidize(Ws,Lambda,P,X); |
---|
74 | |
---|
75 | |
---|
76 | |
---|
77 | |
---|
78 | |
---|
79 | |
---|
80 | |
---|
81 | |
---|