18 lines
360 B
Matlab
18 lines
360 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,:)', -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 |