[37] | 1 | % estimatePX ... estimate the projective shape and motion |
---|
| 2 | % |
---|
| 3 | % [P,X,Lambda] = estimatePX(Ws, Lambda) |
---|
| 4 | % |
---|
| 5 | % Ws ....... 3*nxm measurement matrix |
---|
| 6 | % Lambda ... nxm matrix containg some initial projective depths |
---|
| 7 | % (see also: ESTIMATELAMBDA) |
---|
| 8 | % |
---|
| 9 | % P ........ 3*nx4 matrix containing the projective motion |
---|
| 10 | % X ........ 4xm matrix containing the projective shape |
---|
| 11 | % Lambda ... the new estimation of the projective depths |
---|
| 12 | |
---|
| 13 | function [P,X,Lambda] = estimatePX(Ws, Lambda) |
---|
| 14 | n = size(Ws,1)/3; |
---|
| 15 | m = size(Ws,2); |
---|
| 16 | |
---|
| 17 | % compute 1st updated Ws |
---|
| 18 | for i = 1:n |
---|
| 19 | for j = 1:m |
---|
| 20 | Ws_updated(3*i-2, j) = Ws(3*i-2, j) * Lambda(i, j); |
---|
| 21 | Ws_updated(3*i-1, j) = Ws(3*i-1, j) * Lambda(i, j); |
---|
| 22 | Ws_updated(3*i, j) = Ws(3*i, j) * Lambda(i, j); |
---|
| 23 | end |
---|
| 24 | end |
---|
| 25 | |
---|
| 26 | Lambda_new = Lambda; |
---|
| 27 | iterations = 0; |
---|
| 28 | errs = 1e10*[99.9,99]; |
---|
| 29 | tol = 1e-3; |
---|
| 30 | while (errs(iterations+1)-errs(iterations+2)>tol), |
---|
| 31 | [U,D,V] = svd(Ws_updated); |
---|
| 32 | % the following loop is not needed since these elements of D |
---|
| 33 | % are not considered in further computations |
---|
| 34 | for i = 5:size(D,2) |
---|
| 35 | D(i, i) = 0; |
---|
| 36 | end |
---|
| 37 | |
---|
| 38 | % projective shape X and motion P |
---|
| 39 | P = U*D(1:size(U,2),1:4); |
---|
| 40 | X = V(:,1:4)'; |
---|
| 41 | % U*D*V' == P*X |
---|
| 42 | |
---|
| 43 | % correct projective depths |
---|
| 44 | normfact = sum(P(3:3:(3*n),:)'.^2); |
---|
| 45 | Lambda_old = Lambda_new; |
---|
| 46 | Lambda_new = P(3:3:(3*n),:)./repmat(sqrt(normfact'),1,4)*X; |
---|
| 47 | |
---|
| 48 | % normalize lambdas |
---|
| 49 | lambnfr = sum(Lambda_new.^2); |
---|
| 50 | Lambda_new = sqrt(n)*Lambda_new./repmat(sqrt(lambnfr),n,1); |
---|
| 51 | lambnfc = sum(Lambda_new'.^2); |
---|
| 52 | Lambda_new = sqrt(m)*Lambda_new./repmat(sqrt(lambnfc'),1,m); |
---|
| 53 | |
---|
| 54 | for i = 1:n |
---|
| 55 | for j = 1:m |
---|
| 56 | Ws_updated(3*i-2, j) = Ws(3*i-2, j) * Lambda_new(i, j); |
---|
| 57 | Ws_updated(3*i-1, j) = Ws(3*i-1, j) * Lambda_new(i, j); |
---|
| 58 | Ws_updated(3*i, j) = Ws(3*i, j) * Lambda_new(i, j); |
---|
| 59 | end |
---|
| 60 | end |
---|
| 61 | iterations = iterations + 1; |
---|
| 62 | errs = [errs,sum(sum(abs(Lambda_old-Lambda_new)))]; |
---|
| 63 | % errs(iterations+2) |
---|
| 64 | end |
---|
| 65 | %iterations |
---|
| 66 | |
---|
| 67 | [U,D,V] = svd(Ws_updated); |
---|
| 68 | % compute new projective shape and motion |
---|
| 69 | P = U*D(1:size(U,2),1:4); |
---|
| 70 | X = V(:,1:4)'; |
---|
| 71 | X = X./repmat(X(4,:),4,1); |
---|
| 72 | Lambda = Lambda_new; |
---|
| 73 | |
---|
| 74 | |
---|
| 75 | |
---|