视觉(10)从essential matrix估计r,t.

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

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

Powered by Jekyll and Theme by solid

本站总访问量