

function [PlaneOPT PIopt output]=OptimizeResult(PlaneEST,PIest,T_i,GRIDinfo,intrinsic)



options=optimset('Algorithm','levenberg-marquardt','Display','off','MaxIter',200,...
                 'LevenbergMarquardt','on','TolFun',1E-12,'TolX',1E-12,...
                 'ScaleProblem','none');


clear global IN indexPOINTS Ti
global IN indexPOINTS Ti;
IN=intrinsic;
indexPOINTS=cumsum([GRIDinfo.sizePoints]);
Ti=T_i;

% make x0
d=PIest(4,:);
auxPI=PIest(1:3,:).*[d;d;d];
vecPI=reshape(auxPI,numel(auxPI),1);
vecPlaneEST=[rodrigues(PlaneEST(1:3,1:3)) ; PlaneEST(1:3,4) ];
x0=[vecPlaneEST;vecPI];
% make xdata (3D points) and ydata (2D points)
xdata=[GRIDinfo.X];
ydata=[GRIDinfo.x_px];

[output.x_opt,output.resnorm,output.residual,output.exitflag,output.info,output.lambda,output.jacobian] ...
 = lsqcurvefit(@ObjectiveFunction,x0,xdata,ydata,[],[],options);


PlaneOPT=Rt2T(rodrigues(output.x_opt(1:3)),output.x_opt(4:6));
auxPIopt=output.x_opt(7:end);
auxPIopt=reshape(auxPIopt,3,numel(auxPIopt)/3);
dOPT=sqrt(sum(auxPIopt.^2));
PIopt=[ auxPIopt./[dOPT;dOPT;dOPT] ; dOPT];


