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