36 lines
719 B
Matlab
36 lines
719 B
Matlab
function Q = reconstruction_3D(w1, w2, t_estimee, R_sol)
|
|
|
|
d = size(w1, 1);
|
|
Q = zeros(3, d);
|
|
|
|
for i=1:d
|
|
A = [ -R_sol * w1(i,:)' , w2(i,:)'];
|
|
b = t_estimee;
|
|
|
|
Z = A \ b;
|
|
|
|
Q(:,i) = 1/2 * (Z(2) * w2(i,:)' + R_sol * Z(1) * w1(i,:)' + t_estimee);
|
|
end
|
|
|
|
Q = Q';
|
|
|
|
end
|
|
|
|
% function Q = reconstruction_3D(w1, w2, t_estimee, R_sol)
|
|
%
|
|
% d = size(w1, 1);
|
|
% Q = zeros(3, d);
|
|
%
|
|
% for i=1:d
|
|
% A = [ -R_sol * w1(i,:)' , w2(i,:)', -t_estimee ];
|
|
%
|
|
% [V, D] = eig(A' * A);
|
|
% Z = V(:,1);
|
|
% Z = Z / Z(3);
|
|
%
|
|
% Q(:,i) = 1/2 * (Z(2) * w2(i,:)' + R_sol * Z(1) * w1(i,:)' + t_estimee);
|
|
% end
|
|
%
|
|
% Q = Q';
|
|
%
|
|
% end |