视觉（10）从Essential Matrix估计R,T

%
clc
clear
t = rand( 3 , 1 )
R = rodrigues(rand( 3 , 1 ))
T = [ 0   - t( 3 ) t( 2 );
    t( 3 )  0   - t( 1 );
     - t( 2 ) t( 1 )  0
];

E = T * R
[U,S,V] = svd(E);
disp( ' S?=?diag(1,1,0) ' )
S
W = [ 0   - 1   0 ;
     1   0   0 ;
     0   0   1
];
P1 = [U * W * V '  U(:,3)]
P2 = [U * W ' *V '  U(:, 3 )]
disp( ' check R.. ' )
norm(U * W * V ' -R)
norm(U * W ' *V ' - R)

disp( ' check t.. ' )
norm(U(:, 3 )  -  t / norm(t))
norm( -  U(:, 3 )  -  t / norm(t))