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