视觉（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))