function x= LevMar_BA(X3D, row1,col1,row2,col2, scale_offsets_1,scale_offsets_2, p1_1, p2_1, p3_1, p4_1, p1_2, p2_2, p3_2, p4_2)

npts=length(row1);
for ij=1:npts    
    x1(2,ij) = row1(ij);%(row1(ij)-scale_offsets_1(7))/scale_offsets_1(9);
    x1(1,ij) = col1(ij);%(col1(ij)-scale_offsets_1(8))/scale_offsets_1(10);
    x2(2,ij) = row2(ij);%(row2(ij)-scale_offsets_2(7))/scale_offsets_2(9);
    x2(1,ij) = col2(ij);%(col2(ij)-scale_offsets_2(8))/scale_offsets_2(10);
end

% 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);
    

matches_2D_Inliers = [x1(2,:)', x1(1,:)', x2(2,:)', x2(1,:)'];
param_val = X3D;



%perform minibatches to avoid OOMem
tic
Nbatches=1000;

Len=size(X3D,2);
szbatch=floor(Len/Nbatches);
x = [];
for batch=1:Len
    index = batch;%(batch-1)*szbatch+1:min((batch-1)*szbatch+szbatch,Len);
   
%     noise_perturb = randn(3,1);
    initial_guess=param_val(:,index);
    reference=matches_2D_Inliers(index,:);
    f = @(param)metricdist(param, reference,...
        scale_offsets_1,scale_offsets_2,p1_1,p2_1,p3_1,p4_1,p1_2,p2_2,p3_2,p4_2);
    options = optimoptions(@lsqnonlin,'Display','off', 'Algorithm', 'levenberg-marquardt', 'TolX', 1e-13, 'TolFun', 1e-13);
    [x(:,index),resnorm,residual,EXITFLAG] = lsqnonlin(f,initial_guess, [], [], options);
    
%     x(1,index) = RPCunnormalization(x(1,index),Xo_1,Xs_1);
%     x(2,index) = RPCunnormalization(x(2,index),Yo_1,Ys_1);
%     x(3,index) = RPCunnormalization(x(3,index),Zo_1,Zs_1);
end
toc
end


function F = metricdist(param, matches_2D_Inliers,scale_offsets_1,scale_offsets_2, ...
    p1_1,p2_1,p3_1,p4_1,p1_2,p2_2,p3_2,p4_2)
%        normalizeSpaceCoordinates(rpc, lon,lat,alt)
    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);
  
    for ij=1:size(param,2)
        X1 = RPCnormalization(param(1,ij),Xo_1,Xs_1);
        Y1 = RPCnormalization(param(2,ij),Yo_1,Ys_1);
        Z1 = RPCnormalization(param(3,ij),Zo_1,Zs_1);
        X2 = RPCnormalization(param(1,ij),Xo_2,Xs_2);
        Y2 = RPCnormalization(param(2,ij),Yo_2,Ys_2);
        Z2 = RPCnormalization(param(3,ij),Zo_2,Zs_2);

        numr1(ij,1)=RPCforwardform(p1_1,p2_1,X1,Y1,Z1);
        numc1(ij,1)=RPCforwardform(p3_1,p4_1,X1,Y1,Z1);
        numr2(ij,1)=RPCforwardform(p1_2,p2_2,X2,Y2,Z2);
        numc2(ij,1)=RPCforwardform(p3_2,p4_2,X2,Y2,Z2); 
         
        numr1(ij,1) = RPCunnormalization(numr1(ij,1),scale_offsets_1(7),scale_offsets_1(9));
        numc1(ij,1) = RPCunnormalization(numc1(ij,1),scale_offsets_1(8),scale_offsets_1(10));
        numr2(ij,1) = RPCunnormalization(numr2(ij,1),scale_offsets_2(7),scale_offsets_2(9));
        numc2(ij,1) = RPCunnormalization(numc2(ij,1),scale_offsets_2(8),scale_offsets_2(10));
        
    end

    F = matches_2D_Inliers - [numr1, numc1, numr2, numc2] ;    
end