23 lines
462 B
Mathematica
23 lines
462 B
Mathematica
|
function [t, R] = estimation_pose(t4, R4, w1, w2)
|
||
|
|
|
||
|
|
d = size(w1, 1);
|
||
|
|
Q = zeros(3, d);
|
||
|
|
|
||
|
|
counts = zeros(4, 1);
|
||
|
|
for j=1:4
|
||
|
|
for i=1:d
|
||
|
|
A = [ -R4(:,:,j) * w1(i,:)' , w2(i,:)'];
|
||
|
|
b = t4(:,j);
|
||
|
|
|
||
|
|
Z = A \ b;
|
||
|
|
if Z(1) > 0 && Z(2) > 0
|
||
|
|
counts(j) = counts(j) + 1;
|
||
|
|
end
|
||
|
|
end
|
||
|
|
end
|
||
|
|
|
||
|
|
[~, indice] = max(counts);
|
||
|
|
t = t4(:, indice);
|
||
|
|
R = R4(:, :, indice);
|
||
|
|
|
||
|
|
end
|