23 lines
430 B
Mathematica
23 lines
430 B
Mathematica
|
function [t4, R4] = estimation_4_poses(E)
|
||
|
|
|
||
|
|
% matrices nécéssaires au calculs
|
||
|
|
[U, ~, V] = svd(E);
|
||
|
|
W = [0 -1 0 ; 1 0 0 ; 0 0 1];
|
||
|
|
|
||
|
|
% on calcul t et R au signe près
|
||
|
|
t = U(:,3);
|
||
|
|
R1 = U * W * V';
|
||
|
|
R2 = U * W' * V';
|
||
|
|
|
||
|
|
if det(R1) < 0
|
||
|
|
R1 = -R1;
|
||
|
|
end
|
||
|
|
if det(R2) < 0
|
||
|
|
R2 = -R2;
|
||
|
|
end
|
||
|
|
|
||
|
|
% on assemble les combinaisons
|
||
|
|
t4 = [ t -t t -t];
|
||
|
|
R4 = cat(3, R1, R1, R2, R2);
|
||
|
|
|
||
|
|
end
|