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