branchtriang.m

数学优化工具箱

%%%% Triangulation branching with initial rectangle chosen by initial_bound3.m (limits reprojection error).
%%%% Initializes using initialize2.m and calls boundtriang.m at three places.

function [retX,iternum] = branchtriang(xm, camlist, nbrimages, nbrpoints, alphaval, beta, optim_bound, opts, maxiter, maxdepth)

numvar = 3 + 5*nbrimages;
betainit = beta;
[m,n] = size(camlist{1});

% The convergence ratio of envelope minimum to objective value.
% Typically between 0.90 and 0.99 depending upon the initialization.
% optim_bound = 0.999;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Choosing the initial bounds based on reprojection error
%% opts.autoinit = 1 adjusts value of beta to best possible
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if opts.camnorm == 1
    for camnum = 1:length(camlist)
        tmpcam = camlist{camnum};
        tmpcam = (tmpcam/norm(tmpcam,'fro'))*1000;
        camlist{camnum} = tmpcam;
    end
end
%keyboard;

numerr2flag = 0;
%Hinit = zeros(nbrimages,2);
Hinit = [zeros(nbrimages,1), ones(nbrimages,1)*Inf];
if opts.autoinit == 1
    beta = 2*beta;
    feas_status = 1;
    numinitsteps = -1;
    feascnt = 0;
    while feas_status > 0 & min(Hinit(:,2)-Hinit(:,1)) > 1e-4 & feascnt < 10
        feascnt = feascnt + 1;
        Hinitprev = Hinit;
        numinitsteps = numinitsteps + 1;
        fprintf('Initial bounding : pass %d .... \n', numinitsteps);
        beta = beta/2;
        [Hinit, feas_status] = initial_bound_triang(xm, camlist, nbrimages, nbrpoints, beta, maxdepth, 1);
        if feas_status == 0 & numinitsteps == 0
            numerr2flag = 1;
        end
    end
    %beta = beta * 2;

    feas2 = 1;
    if numerr2flag == 1
        numerr2cnt = 1;
        feas2 = 0;
        while numerr2cnt < 4 & feas2 == 0
            beta = beta/2;
            [Hinit, feas2] = initial_bound_triang(xm, camlist, nbrimages, nbrpoints, beta, maxdepth, 1);
            numerr2cnt = numerr2cnt + 1;
        end
    else
        beta = beta * 2;
        beta = min(beta*nbrimages, betainit);
    end

    [Hinit, feas_status] = initial_bound_triang(xm, camlist, nbrimages, nbrpoints, beta, maxdepth, 0);

    %    if feas == 0
    %        [Hinit, feas] = initial_bound_triang(xm, camlist, nbrimages, nbrpoints, maxdepth, betainit, 1);
    %    end

    if numinitsteps < 1
        fprintf('Initial reprojection error bound too small or large (Sedumi bug).\n\n');
    end
else
    [Hinit, feas_status] = initial_bound_triang(xm, camlist, nbrimages, nbrpoints, betainit, maxdepth, 0);
    %keyboard;
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% The initial reconstruction
rectlist(1).tllist = 0.001*ones(1,nbrimages);
rectlist(1).sllist = Hinit(:,1)';
rectlist(1).tulist = 1000*ones(1,nbrimages);
rectlist(1).sulist = Hinit(:,2)';

%  if feas2 == 0
%    rectlist(1).tllist = 0.001*ones(1,nbrimages);
%    rectlist(1).sllist = 0.001*ones(1,nbrimages);
%    rectlist(1).tulist = 100*ones(1,nbrimages);
%    rectlist(1).sulist = 1000*ones(1,nbrimages);
%  end

if feas_status == 0
    disp('Switching to default initial region.');
    rectlist(1).tllist = 0.001*ones(1,nbrimages);
    rectlist(1).tulist = 100*ones(1,nbrimages);
    rectlist(1).sllist = 0.001*ones(1,nbrimages);
    rectlist(1).sulist = 1000*ones(1,nbrimages);
end

[hlist, lowerbound, objval] = boundtriang(nbrimages, numvar, rectlist(1), xm, camlist);
% keyboard;
envminlist(1) = lowerbound;
objvallist(1) = objval;
Xlast = [hlist.y(1:3); 1];
vopt = objval;
rectcnt = 1;
iternum = 0;
index2arr = zeros(1,nbrimages);

fprintf('\n Initial estimate computed. Starting branch and bound .... \n');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Branch and Bound
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%[mino,mini] = min(objvallist);
%lbmin = envminlist(mini);
%while rectcnt > 0 & (mino-lbmin) > optim_bound

while rectcnt > 0 & min(envminlist ./ vopt) < optim_bound & iternum <= maxiter
    %while rectcnt > 0 & max(envminlist ./ objvallist) < optim_bound
    iternum = iternum + 1;
    %%%%
    %% Rectangle with lowest envelope is chosen to split
    %%%%
    [ytmp, index] = min(envminlist);
    inrect = rectlist(index);

    sLlist = inrect.sllist;
    sUlist = inrect.sulist;
    sdifflist = sUlist - sLlist;

    %%%%
    %% Rectangle "index" is split along longest dimension "index2"
    %%%%
    if opts.onlyn == 1
        [ytmp2, index2] = max(sdifflist(1:n));
    else
        [ytmp2, index2] = max(sdifflist);
    end

    sL = sLlist(index2);
    sU = sUlist(index2);
    border = sL + (sU - sL) * alphaval;
    
    %obj_s = camlist{index2}(3,:)*[hlist(index).y(1:3);1];
    %if (obj_s-sL)/(sU-sL) < alphaval
    %    border = sL + alphaval*(sU-sL);
    %elseif (sU-obj_s)/(sU-sL) < alphaval
    %    border = sU - alphaval*(sU-sL);
    %else
    %    border = obj_s;
    %end
    
    inrect1old = inrect;  inrect2old = inrect;

    %% Reset borders
    inrect1old.sllist(index2) = sL;
    inrect1old.sulist(index2) = border;
    inrect2old.sllist(index2) = border;
    inrect2old.sulist(index2) = sU;


    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% Bound propagation turned on by opts.prop = 1
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if opts.prop == 1 & length(camlist) > n
        inrect1 = propagate_bound_triang(inrect1old, index2, camlist, nbrimages);
        inrect2 = propagate_bound_triang(inrect2old, index2, camlist, nbrimages);
    else
        inrect1 = inrect1old;
        inrect2 = inrect2old;
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    rectlist = [rectlist(1:index-1), inrect1, inrect2, rectlist(index+1:end)];
    rectcnt = rectcnt + 1;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% Bounding
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    [h1new, lowerbound1, objval1] = boundtriang(nbrimages, numvar, inrect1, xm, camlist);
    if vopt > objval1
        vopt = objval1;
        Xlast = [h1new.y(1:3); 1];
        %residual = rmspoints(structure(Xlast),mot,imseq);
        %fprintf('**** New vopt = %2.12f\n', vopt);
    end

    [h2new, lowerbound2, objval2] = boundtriang(nbrimages, numvar, inrect2, xm, camlist);
    if vopt > objval2
        vopt = objval2;
        Xlast = [h2new.y(1:3); 1];
        %residual = rmspoints(structure(Xlast),mot,imseq);
        %fprintf('**** New vopt = %2.12f\n', vopt);
    end

    %% Update lists
    envminlist = [envminlist(1:index-1), lowerbound1, lowerbound2, envminlist(index+1:end)];
    objvallist = [objvallist(1:index-1), objval1, objval2, objvallist(index+1:end)];
    hlist = [hlist(1:index-1), h1new, h2new, hlist(index+1:end)];

    %% Remove rectangles with envelope minimum above current vopt
    indretain = find(envminlist < vopt);
    rectcnt = length(indretain);
    if rectcnt == 0
        disp('No rectangles retained.')
    end

    %% Update lists
    rectlist = rectlist(indretain);
    envminlist = envminlist(indretain);
    objvallist = objvallist(indretain);
    hlist = hlist(indretain);


    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% Output display
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %fprintf('Ground Truth = %2.6f %2.6f %2.6f %2.6f\n', ground_truth(1), ground_truth(2), ground_truth(3), ground_truth(4));

    fprintf('Iteration: %d  ', iternum);
    fprintf('Rectangles : %d  ', rectcnt);
    %    fprintf('Reconstruction : %f ', Xlast);
    %fprintf('Iteration: %d\t vopt: %2.12f\t Residual = %2.12f\n', iternum, vopt, residual);
    fprintf('  vopt:  %f  Optimality status: %2.6f\t Required: %2.6f\n', vopt, min(envminlist ./ vopt), optim_bound);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
end;

retX = Xlast(1:3);
fprintf('Number of iterations : %d\n', iternum);