1 | function V = find_rigid(Mapprox,r) |
---|
2 | %NOTE: ASSUMES RANK 3. WILL NOT WORK FOR OTHER VALUES OF r. |
---|
3 | |
---|
4 | % extract out the tranformation component, in U |
---|
5 | % the structure is then in V |
---|
6 | |
---|
7 | [Ui,Si,Vi] = svd(Mapprox); |
---|
8 | %V = Vi(:,1:r)*diag(sqrt(diag(Si(1:r,1:r)))); |
---|
9 | %U = Ui(:,1:r)*diag(sqrt(diag(Si(1:r,1:r)))); |
---|
10 | V = Vi(:,1:r)*Si(1:r,1:r); |
---|
11 | U = Ui(:,1:r); |
---|
12 | u_rows = size(U,1); |
---|
13 | |
---|
14 | for i = 1:u_rows |
---|
15 | Y(i,1:3) = U(i,1)*U(i,:); |
---|
16 | Y(i,4:6) = U(i,2)*U(i,:); |
---|
17 | Y(i,7:9) = U(i,3)*U(i,:); |
---|
18 | X(i) = 1; |
---|
19 | end |
---|
20 | |
---|
21 | for i = 1 : u_rows/2 |
---|
22 | Y(u_rows+i,1:3) = U(2*i-1,1)*U(2*i,:); |
---|
23 | Y(u_rows+i,4:6) = U(2*i-1,2)*U(2*i,:); |
---|
24 | Y(u_rows+i,7:9) = U(2*i-1,3)*U(2*i,:); |
---|
25 | X(u_rows+i) = 0; |
---|
26 | end |
---|
27 | |
---|
28 | %constrain A'A to be symmetric |
---|
29 | Y = [Y(:,1) Y(:,2)+Y(:,4) Y(:,3)+Y(:,7) Y(:,5) Y(:,6)+Y(:,8) Y(:,9)]; |
---|
30 | |
---|
31 | %use least squares to find A'A |
---|
32 | X = lscov(Y,X',eye(u_rows+u_rows/2)); |
---|
33 | |
---|
34 | % reshape A'A into a 3x3 matrix |
---|
35 | Z(1,1:3) = X(1:3)'; |
---|
36 | Z(2,2:3) = X(4:5)'; |
---|
37 | Z(1:3,1) = X(1:3); |
---|
38 | Z(3,2) = Z(2,3); |
---|
39 | Z(3,3) = X(6); |
---|
40 | |
---|
41 | %Z now contains A'A, where A is the affine xform that minimizes the |
---|
42 | %distance to a rigid transform |
---|
43 | [U2,S,V2] = svd(Z); |
---|
44 | |
---|
45 | A = U2 * diag(sqrt(diag(S))); |
---|
46 | %A now contains the correct affine xform. |
---|
47 | |
---|
48 | %and UA is the closest rigid transform to U |
---|
49 | V = V*inv(A)'; |
---|
50 | |
---|
51 | |
---|