solvePnP_from_P3P_norm.m

function [R,T]=solvePnP_from_P3P_norm(object_points, image_points_norm)


n=0;

for i=1:size(object_points)
   
    n=n+1;
    PP(n,:)=[image_points_norm(i,1),image_points_norm(i,2),object_points(i,1),object_points(i,2),object_points(i,3)];

end

if n>3

    [R3,T3]=solveP3P_norm(object_points, image_points_norm);

end

min=inf;

for i=1:size(R3,2)
    
    error=0;
    
    for j=1:size(PP,1)
        
         UVtemp=[R3{i},T3{i}]*[PP(j,3:5)';1];
         
         error=error+norm(UVtemp(1:2)/UVtemp(3)-PP(j,1:2)');
        
    end
    
    if error < min 
        
        min=error;
        
        R_=R3{i};
        
        T_=T3{i};
        
    end
 
end


[R_,T_]=My_GaussNewton_PnP(PP,R_,T_);


R=R_';

T=-R_'*T_;



end



function [R,T]=My_GaussNewton_PnP(PP,Rinit,Tinit)

q=rotMat2qRichard(Rinit);

phi=quaternionToAngleAxis(q);


X=[phi;Tinit];


for i=1:size(PP,1)
     
     UV(i*2-1,1)=PP(i,1);
        
     UV(i*2-0,1)=PP(i,2);
    
end


for i=1:6
    
    [UVn,Jacbi]=f_PnP(X,PP);
    
    %UV-UVn
    
    X=X+inv(Jacbi'*Jacbi)*Jacbi'*(UV-UVn);
    
end

R=angleAxisToRotationMatrix(X(1:3));

T=X(4:6);

end



function [UVn,Jacbi]=f_PnP(X,PP)

angle=norm(X(1:3));

if angle==0
    
    angle_axis=[1;0;0];
    
    Jl=eye(3);
    
else
    
    angle_axis=X(1:3)/angle;
    
    Jl=sin(angle)/angle*eye(3)+(1-sin(angle)/angle)*angle_axis*angle_axis'+(1-cos(angle))/angle*Skew_symmetric(angle_axis); 
    
end
    
R=angleAxisToRotationMatrix(X(1:3));

T=X(4:6);

UVn=zeros(size(PP,1)*2,1);

Jacbi=zeros(size(PP,1)*2,6);

for i=1:size(PP,1)
    
    P=PP(i,3:5)';
    
    Vtemp=R*P+T;
    
    UVn(i*2-1,1)= Vtemp(1)/ Vtemp(3);
    
    UVn(i*2-0,1)= Vtemp(2)/ Vtemp(3);
    
    d_UV_d_Vtemp=[1/Vtemp(3),     0    ,- Vtemp(1)/ Vtemp(3)^2;...
                       0    ,1/Vtemp(3),- Vtemp(2)/ Vtemp(3)^2];
                   
    d_Vtemp_d_X=[ -Skew_symmetric(R*P)*Jl,eye(3)];
    
    Jacbi(i*2-1:i*2,:)=d_UV_d_Vtemp*d_Vtemp_d_X;
    
end





end