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