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