function [R, t] = objpose2(P_w, v_im, options)
%P_w is a 3xn matrix of world locations
%v_im is a 3xn matrix of camera-frame vectors from the image space.

n=size(P_w,2);
V=zeros(3,3,n);
to_inv=zeros(3);

%Form the projection matrices
for ii=1:n,
    V(:,:,ii)=(v_im(:,ii)*v_im(:,ii).')/(v_im(:,ii).'*v_im(:,ii));
    to_inv=to_inv+V(:,:,ii);
end
%This matrix is used to find the optimal T for a given R
to_inv=eye(3)-to_inv/n;
to_find_t=inv(to_inv)/n;

if isfield(options, 'initR')
    curr_R=options.initR;
    if isfield(options,'initT')
        curr_T=options.initT;
    else
        curr_T=-curr_R*mean(P_w,2);
    end
else
	[curr_R,curr_T]=best_Rot_and_trans(P_w,v_im,V,to_find_t);
end

for ii=1:50 %50 is just a random number for the number of iterations
    %Move coordinate frames from world to camera
    P_in_cam_frame=curr_R*P_w+curr_T*ones(1,n);
    %Find Q which is the projection of P_in_cam_frame onto the vectors
    for jj=1:n,
        Q(:,jj)=V(:,:,jj)*P_in_cam_frame(:,jj);
    end
    tmp=Q-P_in_cam_frame;
    curr_cost=sum(sum(tmp.*tmp))
    if (curr_cost == 0)
        'Terminating due to cost of 0'
        ii
        ii=50;
    end
    [curr_R,curr_T]=best_Rot_and_trans(P_w,Q,V,to_find_t);
end

R=curr_R;
t=curr_T;

function [R,t] = best_Rot_and_trans (P,Q,V,to_find_t)
%I assume P and Q are 3xn matrices, where n is the same for both matrices

n=size(P,2);
A=zeros(3);
p_bar=mean(P,2);
q_bar=mean(Q,2);
P=P-p_bar*ones(1,n);
Q=Q-q_bar*ones(1,n);
%Mean of points has been removed.  Now to form the matrix of outer products
for ii=1:n,
    A=A+P(:,ii)*Q(:,ii).';
end

[u,s,v]=svd(A);

if det(u)<0
    u(3,:)=-u(3,:);
end
if det(v)<0
    v(3,:)=-v(3,:);
end
R=v*u.';

%Use the optimal R to find the best t
to_find_t_vect=[0;0;0];
for ii=1:n,
    to_find_t_vect=to_find_t_vect + V(:,:,ii)*R*P(:,ii);
end
t=to_find_t*to_find_t_vect - R*p_bar;