function [Xu,Yu,Zu,error3d,error2d] = triangulationRPC_BA(p1_1,p2_1,p3_1,p4_1,p1_2,p2_2,p3_2,p4_2,PDelta1,PDelta2,ru1,cu1,ru2,cu2,scale_offsets_1,scale_offsets_2,verbose)


    %X~long
    %Y~lat
    %Z~Hei
    
 
error3d=10e10;
if nargout>4
    error2d=10e10;
end  

    
    %input r1,c1,r2,c2 need to be with respect of the global image (not
    %cropped versions, offsets won't align to it)
% ru1=14758+1036.6;
% cu1=14718+52.96;
% ru2=13418+1048.1;
% cu2=14234+24.42;
    r1 = (ru1-scale_offsets_1(7))/scale_offsets_1(9);
    c1 = (cu1-scale_offsets_1(8))/scale_offsets_1(10);
    r2 = (ru2-scale_offsets_2(7))/scale_offsets_2(9);
    c2 = (cu2-scale_offsets_2(8))/scale_offsets_2(10);
    

    %convention
    % X~lon, Y~lat, Z~Height
    %setup Parameters with notation
    Xs_1=scale_offsets_1(3);Xo_1=scale_offsets_1(4);
    Ys_1=scale_offsets_1(1);Yo_1=scale_offsets_1(2);
    Zs_1=scale_offsets_1(5);Zo_1=scale_offsets_1(6);
    Xs_2=scale_offsets_2(3);Xo_2=scale_offsets_2(4);
    Ys_2=scale_offsets_2(1);Yo_2=scale_offsets_2(2);
    Zs_2=scale_offsets_2(5);Zo_2=scale_offsets_2(6);

    %1st iteration only linear model:
%     vr1=0;vc1=0;vr2=0;vc2=0;
    %camera 1
    %row
    a0_1=p1_1(1)*Zs_1*Ys_1*Xs_1 - p1_1(4)*Zo_1*Ys_1*Xs_1-p1_1(3)*Zs_1*Yo_1*Xs_1-p1_1(2)*Zs_1*Ys_1*Xo_1;
    a1_1=p1_1(4)*Ys_1*Xs_1;a2_1=p1_1(3)*Zs_1*Xs_1;a3_1=p1_1(2)*Zs_1*Ys_1;
    b0_1=p2_1(1)*Zs_1*Ys_1*Xs_1-p2_1(4)*Zo_1*Ys_1*Xs_1-p2_1(3)*Zs_1*Yo_1*Xs_1-p2_1(2)*Zs_1*Ys_1*Xo_1;
    b1_1=p2_1(4)*Ys_1*Xs_1;b2_1=p2_1(3)*Zs_1*Xs_1;b3_1=p2_1(2)*Zs_1*Ys_1;
    %col
    c0_1=p3_1(1)*Zs_1*Ys_1*Xs_1 - p3_1(4)*Zo_1*Ys_1*Xs_1-p3_1(3)*Zs_1*Yo_1*Xs_1-p3_1(2)*Zs_1*Ys_1*Xo_1;
    c1_1=p3_1(4)*Ys_1*Xs_1;c2_1=p3_1(3)*Zs_1*Xs_1;c3_1=p3_1(2)*Zs_1*Ys_1;
    d0_1=p4_1(1)*Zs_1*Ys_1*Xs_1-p4_1(4)*Zo_1*Ys_1*Xs_1-p4_1(3)*Zs_1*Yo_1*Xs_1-p4_1(2)*Zs_1*Ys_1*Xo_1;
    d1_1=p4_1(4)*Ys_1*Xs_1;d2_1=p4_1(3)*Zs_1*Xs_1;d3_1=p4_1(2)*Zs_1*Ys_1;
    %camera 2
    %row
    a0_2=p1_2(1)*Zs_2*Ys_2*Xs_2 - p1_2(4)*Zo_2*Ys_2*Xs_2-p1_2(3)*Zs_2*Yo_2*Xs_2-p1_2(2)*Zs_2*Ys_2*Xo_2;
    a1_2=p1_2(4)*Ys_2*Xs_2;a2_2=p1_2(3)*Zs_2*Xs_2;a3_2=p1_2(2)*Zs_2*Ys_2;
    b0_2=p2_2(1)*Zs_2*Ys_2*Xs_2-p2_2(4)*Zo_2*Ys_2*Xs_2-p2_2(3)*Zs_2*Yo_2*Xs_2-p2_2(2)*Zs_2*Ys_2*Xo_2;
    b1_2=p2_2(4)*Ys_2*Xs_2;b2_2=p2_2(3)*Zs_2*Xs_2;b3_2=p2_2(2)*Zs_2*Ys_2;
    %col
    c0_2=p3_2(1)*Zs_2*Ys_2*Xs_2 - p3_2(4)*Zo_2*Ys_2*Xs_2-p3_2(3)*Zs_2*Yo_2*Xs_2-p3_2(2)*Zs_2*Ys_2*Xo_2;
    c1_2=p3_2(4)*Ys_2*Xs_2;c2_2=p3_2(3)*Zs_2*Xs_2;c3_2=p3_2(2)*Zs_2*Ys_2;
    d0_2=p4_2(1)*Zs_2*Ys_2*Xs_2-p4_2(4)*Zo_2*Ys_2*Xs_2-p4_2(3)*Zs_2*Yo_2*Xs_2-p4_2(2)*Zs_2*Ys_2*Xo_2;
    d1_2=p4_2(4)*Ys_2*Xs_2;d2_2=p4_2(3)*Zs_2*Xs_2;d3_2=p4_2(2)*Zs_2*Ys_2;
    %note: this system eqs assume (Z,Y,X ordering) as in seminal paper (oNLY
    %ITERATION 1)
    A=[a1_1-r1*b1_1 a2_1-r1*b2_1 a3_1-r1*b3_1;...
        c1_1-c1*d1_1 c2_1-c1*d2_1 c3_1-c1*d3_1; ...
        a1_2-r2*b1_2 a2_2-r2*b2_2 a3_2-r2*b3_2;...
        c1_2-c2*d1_2 c2_2-c2*d2_2 c3_2-c2*d3_2];
    b=[r1*b0_1-a0_1;c1*d0_1-c0_1;r2*b0_2-a0_2;c2*d0_2-c0_2];
    
    % LSsol=inv(A'*A)*A'*b;
    LSsol=A\b;
    
    DeltaXu=LSsol(3);DeltaYu=LSsol(2);DeltaZu=LSsol(1);
    %next iterations
    Niter=50;
    NormXold=0;NormYold=0;NormZold=0;
    Xunew_1=DeltaXu;Yunew_1=DeltaYu;Zunew_1=DeltaZu;
    Xunew_2=DeltaXu;Yunew_2=DeltaYu;Zunew_2=DeltaZu;    
    NormupdateX_1=RPCnormalization(DeltaXu,Xo_1,Xs_1);    
    NormupdateY_1=RPCnormalization(DeltaYu,Yo_1,Ys_1);
    NormupdateZ_1=RPCnormalization(DeltaZu,Zo_1,Zs_1);
    NormupdateX_2=RPCnormalization(DeltaXu,Xo_2,Xs_2);
    NormupdateY_2=RPCnormalization(DeltaYu,Yo_2,Ys_2);
    NormupdateZ_2=RPCnormalization(DeltaZu,Zo_2,Zs_2);
    NormXnew_1=NormXold+NormupdateX_1;
    NormYnew_1=NormYold+NormupdateY_1;
    NormZnew_1=NormZold+NormupdateZ_1;
    NormXnew_2=NormXold+NormupdateX_2;
    NormYnew_2=NormYold+NormupdateY_2;
    NormZnew_2=NormZold+NormupdateZ_2;
    error_residual_old=10e10;
    cnt=0;
    for it=1:Niter
        %system eqs
        %partial derivatives
        %1st camera
        %input needs to be normalized but the algorithm runs with
        %unnormalized updates (derivatives chain rule)
        pol1 = RPCsinglepolynomial(p1_1,NormXnew_1,NormYnew_1,NormZnew_1);
        pol2 = RPCsinglepolynomial(p2_1,NormXnew_1,NormYnew_1,NormZnew_1);
        pol3 = RPCsinglepolynomial(p3_1,NormXnew_1,NormYnew_1,NormZnew_1);
        pol4 = RPCsinglepolynomial(p4_1,NormXnew_1,NormYnew_1,NormZnew_1);
        pol1x = RPCsinglepolynomialderivativeX(p1_1,NormXnew_1,NormYnew_1,NormZnew_1);
        pol1y = RPCsinglepolynomialderivativeY(p1_1,NormXnew_1,NormYnew_1,NormZnew_1);
        pol1z = RPCsinglepolynomialderivativeZ(p1_1,NormXnew_1,NormYnew_1,NormZnew_1);
        pol2x = RPCsinglepolynomialderivativeX(p2_1,NormXnew_1,NormYnew_1,NormZnew_1);
        pol2y = RPCsinglepolynomialderivativeY(p2_1,NormXnew_1,NormYnew_1,NormZnew_1);
        pol2z = RPCsinglepolynomialderivativeZ(p2_1,NormXnew_1,NormYnew_1,NormZnew_1);
        pol3x = RPCsinglepolynomialderivativeX(p3_1,NormXnew_1,NormYnew_1,NormZnew_1);
        pol3y = RPCsinglepolynomialderivativeY(p3_1,NormXnew_1,NormYnew_1,NormZnew_1);
        pol3z = RPCsinglepolynomialderivativeZ(p3_1,NormXnew_1,NormYnew_1,NormZnew_1);
        pol4x = RPCsinglepolynomialderivativeX(p4_1,NormXnew_1,NormYnew_1,NormZnew_1);
        pol4y = RPCsinglepolynomialderivativeY(p4_1,NormXnew_1,NormYnew_1,NormZnew_1);
        pol4z = RPCsinglepolynomialderivativeZ(p4_1,NormXnew_1,NormYnew_1,NormZnew_1);
        pdXrow_1 = (pol1x*pol2-pol1*pol2x)/(pol2^2);
        pdYrow_1 = (pol1y*pol2-pol1*pol2y)/(pol2^2);
        pdZrow_1 = (pol1z*pol2-pol1*pol2z)/(pol2^2);
        pdXcol_1 = (pol3x*pol4-pol3*pol4x)/(pol4^2);
        pdYcol_1 = (pol3y*pol4-pol3*pol4y)/(pol4^2);
        pdZcol_1 = (pol3z*pol4-pol3*pol4z)/(pol4^2);
        %2nd camera
        pol1 = RPCsinglepolynomial(p1_2,NormXnew_2,NormYnew_2,NormZnew_2);
        pol2 = RPCsinglepolynomial(p2_2,NormXnew_2,NormYnew_2,NormZnew_2);
        pol3 = RPCsinglepolynomial(p3_2,NormXnew_2,NormYnew_2,NormZnew_2);
        pol4 = RPCsinglepolynomial(p4_2,NormXnew_2,NormYnew_2,NormZnew_2);
        pol1x = RPCsinglepolynomialderivativeX(p1_2,NormXnew_2,NormYnew_2,NormZnew_2);
        pol1y = RPCsinglepolynomialderivativeY(p1_2,NormXnew_2,NormYnew_2,NormZnew_2);
        pol1z = RPCsinglepolynomialderivativeZ(p1_2,NormXnew_2,NormYnew_2,NormZnew_2);
        pol2x = RPCsinglepolynomialderivativeX(p2_2,NormXnew_2,NormYnew_2,NormZnew_2);
        pol2y = RPCsinglepolynomialderivativeY(p2_2,NormXnew_2,NormYnew_2,NormZnew_2);
        pol2z = RPCsinglepolynomialderivativeZ(p2_2,NormXnew_2,NormYnew_2,NormZnew_2);
        pol3x = RPCsinglepolynomialderivativeX(p3_2,NormXnew_2,NormYnew_2,NormZnew_2);
        pol3y = RPCsinglepolynomialderivativeY(p3_2,NormXnew_2,NormYnew_2,NormZnew_2);
        pol3z = RPCsinglepolynomialderivativeZ(p3_2,NormXnew_2,NormYnew_2,NormZnew_2);
        pol4x = RPCsinglepolynomialderivativeX(p4_2,NormXnew_2,NormYnew_2,NormZnew_2);
        pol4y = RPCsinglepolynomialderivativeY(p4_2,NormXnew_2,NormYnew_2,NormZnew_2);
        pol4z = RPCsinglepolynomialderivativeZ(p4_2,NormXnew_2,NormYnew_2,NormZnew_2);
        pdXrow_2 = (pol1x*pol2-pol1*pol2x)/(pol2^2);
        pdYrow_2 = (pol1y*pol2-pol1*pol2y)/(pol2^2);
        pdZrow_2 = (pol1z*pol2-pol1*pol2z)/(pol2^2);
        pdXcol_2 = (pol3x*pol4-pol3*pol4x)/(pol4^2);
        pdYcol_2 = (pol3y*pol4-pol3*pol4y)/(pol4^2);
        pdZcol_2 = (pol3z*pol4-pol3*pol4z)/(pol4^2);
        %build Jacobian A:
        A = [pdXrow_1/Xs_1 pdYrow_1/Ys_1 pdZrow_1/Zs_1;...
            pdXcol_1/Xs_1 pdYcol_1/Ys_1 pdZcol_1/Zs_1;...
            pdXrow_2/Xs_2 pdYrow_2/Ys_2 pdZrow_2/Zs_2;...
            pdXcol_2/Xs_2 pdYcol_2/Ys_2 pdZcol_2/Zs_2];
        r1_hat = RPCforwardform(p1_1,p2_1,NormXnew_1,NormYnew_1,NormZnew_1)+PDelta1(1);
        c1_hat = RPCforwardform(p3_1,p4_1,NormXnew_1,NormYnew_1,NormZnew_1)+PDelta1(2);
        r2_hat = RPCforwardform(p1_2,p2_2,NormXnew_2,NormYnew_2,NormZnew_2)+PDelta2(1);
        c2_hat = RPCforwardform(p3_2,p4_2,NormXnew_2,NormYnew_2,NormZnew_2)+PDelta2(2);
        
        b = [r1-r1_hat;c1-c1_hat;r2-r2_hat;c2-c2_hat];
        %bb=b.*[scale_offsets_1(9);scale_offsets_1(10);scale_offsets_2(9);scale_offsets_2(10)];
        
        %solution        
        LSsol=A\b;
        DeltaXu=LSsol(1);DeltaYu=LSsol(2);DeltaZu=LSsol(3);
        %update unnormalized
        Xuold = Xunew_1;Yuold = Yunew_1;Zuold = Zunew_1; 
        
        Xunew_1 = Xunew_1 + DeltaXu;
        Yunew_1 = Yunew_1 + DeltaYu;
        Zunew_1 = Zunew_1 + DeltaZu;
        Xunew_2 = Xunew_2 + DeltaXu;
        Yunew_2 = Yunew_2 + DeltaYu;
        Zunew_2 = Zunew_2 + DeltaZu;
        error_residual = sqrt(DeltaXu^2+DeltaYu^2+DeltaZu^2);        
        %normalize updated coordinates
        NormXnew_1=RPCnormalization(Xunew_1,Xo_1,Xs_1);
        NormYnew_1=RPCnormalization(Yunew_1,Yo_1,Ys_1);
        NormZnew_1=RPCnormalization(Zunew_1,Zo_1,Zs_1);
        NormXnew_2=RPCnormalization(Xunew_2,Xo_2,Xs_2);
        NormYnew_2=RPCnormalization(Yunew_2,Yo_2,Ys_2);
        NormZnew_2=RPCnormalization(Zunew_2,Zo_2,Zs_2);
         %convert degrees to UTM (m)
        [Yunew_1_m,Xunew_1_m,utmzone] = deg2utm(Yunew_1, Xunew_1);
        [Yuold_m,Xuold_m,utmzone] = deg2utm(Yuold, Xuold);
        error_residual_m = sqrt((Xuold_m-Xunew_1_m)^2+(Yuold_m-Yunew_1_m)^2+DeltaZu^2);
        if nargout>4
            %error calculation
            r1_est =  RPCforwardform(p1_1,p2_1,NormXnew_1,NormYnew_1,NormZnew_1)+PDelta1(1);
            c1_est =  RPCforwardform(p3_1,p4_1,NormXnew_1,NormYnew_1,NormZnew_1)+PDelta1(2);
            r2_est =  RPCforwardform(p1_2,p2_2,NormXnew_2,NormYnew_2,NormZnew_2)+PDelta2(1);
            c2_est =  RPCforwardform(p3_2,p4_2,NormXnew_2,NormYnew_2,NormZnew_2)+PDelta2(2);
            %unnormalize        
            r1_est = RPCunnormalization(r1_est,scale_offsets_1(7),scale_offsets_1(9));
            c1_est = RPCunnormalization(c1_est,scale_offsets_1(8),scale_offsets_1(10));
            r2_est = RPCunnormalization(r2_est,scale_offsets_2(7),scale_offsets_2(9));
            c2_est = RPCunnormalization(c2_est,scale_offsets_2(8),scale_offsets_2(10));


            error_pixel_residual_1 = sqrt((ru1-r1_est)^2+(cu1-c1_est)^2);
            error_pixel_residual_2 = sqrt((ru2-r2_est)^2+(cu2-c2_est)^2);
         end
        if verbose
            fprintf('previous 3D point:%.10f,%.10f,%.10f updatedCAM1:%.10f,%.10f,%.10f updatedCAM2:%.10f,%.10f,%.10f \n',...
                Xuold,Yuold,Zuold,Xunew_1,Yunew_1,Zunew_1,Xunew_2,Yunew_2,Zunew_2);
            fprintf('original pixel pointCAM1:%.10f,%.10f updatedCAM1:%.10f,%.10f \n',...
                ru1,cu1,r1_est,c1_est);
            fprintf('original pixel pointCAM2:%.10f,%.10f updatedCAM2:%.10f,%.10f \n',...
                ru2,cu2,r2_est,c2_est);
            fprintf('Unnorm error3D:%.15f DeltaYu_m:%.10f DeltaXu_m:%.10f DeltaZu:%.10f at iter %d\n',...
                error_residual_m,abs(Xuold_m-Xunew_1_m),abs(Yuold_m-Yunew_1_m),DeltaZu,it);
            fprintf('Unnorm error_residual_m:%.15f error_pixel_residual_1:%.10f error_pixel_residual_2:%.10f at iter %d\n',...
                error_residual,error_pixel_residual_1,error_pixel_residual_2,it);
        end
        %if error_pixel_residual_1*0.5+error_pixel_residual_2*0.5<10e-3
        % when error is smaller than 1e-10 meters (1 minute of Lon,lat~2km)
        Xu = Xunew_1*0.5+Xunew_2*0.5;
        Yu = Yunew_1*0.5+Yunew_2*0.5;
        Zu = Zunew_1*0.5+Zunew_2*0.5;
%          Xu = min(Xunew_1,Xunew_2);
%          Yu = min(Yunew_1,Yunew_2);
%          Zu = min(Zunew_1,Zunew_2);

        
       
        cnt=cnt+1;
        if error_residual*60<(1/2000)*1e-6
            error3d=error_residual;
            if nargout>4
                error2d=error_pixel_residual_1*0.5+error_pixel_residual_2*0.5;
            end
            break;
%         elseif error_residual/error_residual_old>0.99
%             if verbose
%                 'convergence'            
%             end
%             error3d=error_residual;
%             if nargout>4
%                 error2d=error_pixel_residual_1*0.5+error_pixel_residual_2*0.5;
%             end
%             break;
        end
        error_residual_old=error_residual;
        
    end
    
end

function [normdata]=RPCnormalization(undata,offset,scale)
    normdata=(undata-offset)/scale;
end

function [undata]=RPCunnormalization(normdata,offset,scale)
    undata=normdata*scale+offset;
end

function [sol] = RPCsinglepolynomialderivativeX(p,X,Y,Z)
   sol=0;
   %monomial=[1 X Y Z X*Y X*Z Y*Z X^2 Y^2 Z^2 X*Y*Z X^3 X*Y^2 X*Z^2 X^2*Y ...
   %    Y^3 Y*Z^2 X^2*Z Y^2*Z Z^3];
   monomial=[0 1 0 0 Y Z 0 2*X 0 0 Y*Z 3*X^2 Y^2 Z^2 2*X*Y ...
       0 0 2*X*Z 0 0];
   for i=1:length(monomial)
       sol=sol+monomial(i)*p(i);
   end
end
   
function [sol] = RPCsinglepolynomialderivativeY(p,X,Y,Z)
   sol=0;
   %monomial=[1 X Y Z X*Y X*Z Y*Z X^2 Y^2 Z^2 X*Y*Z X^3 X*Y^2 X*Z^2 X^2*Y ...
   %    Y^3 Y*Z^2 X^2*Z Y^2*Z Z^3];
   monomial=[0 0 1 0 X 0 Z 0 2*Y 0 X*Z 0 X*2*Y 0 X^2 ...
       3*Y^2 Z^2 0 2*Y*Z 0];
   for i=1:length(monomial)
       sol=sol+monomial(i)*p(i);
   end
end
   
function [sol] = RPCsinglepolynomialderivativeZ(p,X,Y,Z)
   sol=0;
   %monomial=[1 X Y Z X*Y X*Z Y*Z X^2 Y^2 Z^2 X*Y*Z X^3 X*Y^2 X*Z^2 X^2*Y ...
   %    Y^3 Y*Z^2 X^2*Z Y^2*Z Z^3];
   monomial=[0 0 0 1 0 X Y 0 0 2*Z X*Y 0 0 X*2*Z 0 ...
       0 Y*2*Z X^2 Y^2 3*Z^2];
   for i=1:length(monomial)
       sol=sol+monomial(i)*p(i);
   end
end

function [sol] = RPCsinglepolynomial(p,X,Y,Z)
    sol=0;
    monomial=[1 X Y Z X*Y X*Z Y*Z X^2 Y^2 Z^2 X*Y*Z X^3 X*Y^2 X*Z^2 X^2*Y ...
       Y^3 Y*Z^2 X^2*Z Y^2*Z Z^3];
   for i=1:length(monomial)
       sol=sol+monomial(i)*p(i);
   end
end
   
function [pixel_coordinate] = RPCforwardform(p,q,X,Y,Z)
    num=0;den=0;
    monomial=[1 X Y Z X*Y X*Z Y*Z X^2 Y^2 Z^2 X*Y*Z X^3 X*Y^2 X*Z^2 X^2*Y ...
       Y^3 Y*Z^2 X^2*Z Y^2*Z Z^3];
    for i=1:length(monomial)
        num=num+p(i)*monomial(i);
        den=den+q(i)*monomial(i);
    end
    if den==0
        den=1;
    end
    pixel_coordinate=num/den;
end