function [p,p_lp] = addSymmetryCuts(p,p_lp)

for j = 1:length(p.sdpsymmetry)
    if length(p.sdpsymmetry{j}.variables{1}) <= 4
        % We can enumerate easily infeasibles
        excludes = [];
        n = sqrt(size(p.sdpsymmetry{j}.dataBlock,1));
        vars = p.sdpsymmetry{j}.variables{1};
        % Now enumerate, for historical reasons
        % also accept {-1,0}-models so translate and scale
        L = p.lb(vars);
        U = p.ub(vars);
        m = length(vars);
        combs = dec2decbin(0:2^m-1,m)';
        combs = repmat(L,1,size(combs,2)) + repmat(U-L,1,size(combs,2)).*combs;
        combs = unique(combs','rows')';
        feasible = ones(1,size(combs,2));
        for i = 1:size(combs,2)
            if min(eig(reshape(p.sdpsymmetry{j}.dataBlock*[1;combs(:,i)],n,n))) < -abs(p_lp.options.bnb.feastol)
                excludes = [excludes combs(:,i)];
                feasible(i) = 0;
            end
        end
        % We can model the inconsistencies in various ways
        % 1. Binary variables sum less
        % 2. Binary variables sum larger
        % 3. Generic, just exclude
        if ~isempty(excludes)
            done = 0;
            sum_feas = sum(combs(:,find(feasible)),1);
            sum_infeas = sum(excludes,1);
            newF = [];
            if all(sum_infeas==sum_infeas(1)) && all(sum_feas < sum_infeas(1) | sum_feas > sum_infeas(1))
                % There is a forbidden sum among these
                for s = 1:length(p.sdpsymmetry{j}.variables)
                    p.forbiddencardinality.variables{end+1} = p.sdpsymmetry{j}.variables{s};
                    p.forbiddencardinality.value{end+1} = sum_infeas(1);
                end
            end
            for k = min(sum(combs,1)):max(sum(combs,1))
                if all(sum_feas<=k) && all(sum_infeas>k)
                    for s = 1:length(p.sdpsymmetry{j}.variables)
                        a = spalloc(1,length(p_lp.c),1);
                        a(p.sdpsymmetry{j}.variables{s}) = -1;
                        newF = [newF;k a];
                    end
                    done = 1;
                    break
                elseif  all(sum_feas>=k) && all(sum_feas<k)
                    for s = 1:length(p.sdpsymmetry{j}.variables)
                        a = spalloc(1,length(p_lp.c),1);
                        a(p.sdpsymmetry{j}.variables{s}) = 1;
                        newF = [newF;-k a];
                    end
                    done = 1;
                    break
                end
            end
            if ~done
                % No good structure, just add no-goods
                infeasible_combinations = unique(excludes','rows')';
                for k = 1:size(infeasible_combinations,2)
                    % Local cut for reduced set
                    if min(min(excludes))==-1
                        binary_type = -1;
                    else
                        binary_type = 1;
                    end
                    [b,atemp] = exclusionCut(infeasible_combinations(:,k),binary_type);
                    % Add that cut for every variable groups
                    for s = 1:length(p.sdpsymmetry{j}.variables)
                        a = spalloc(1,length(p_lp.c),1);
                        a(p.sdpsymmetry{j}.variables{s}) = atemp;
                        newF = [newF;b a];
                    end
                end
            end
            p_lp = addInequality(p_lp,newF);
            % Delete, won't need these in the future
            p.sdpsymmetry{j}.dataBlock = [];
            p.sdpsymmetry{j}.variables = [];
        end
    end
end


function [gp, diagnosis] = svigp(gp, x, y, varargin)
%SVIGP  Stochastic variational inference for GP
%
%  Description
%    GP = SVIGP(GP, X, Y, OPTIONS) optimises the variational, likelihood
%    and covariance function parameters of a sparse SVI GP model
%    given matrix X of training inputs and vector Y of training targets.
%    The model follows the description in Hensman, Fusi and Lawrence
%    (2013). Supported likelihood functions are gaussian (for regression)
%    and probit (for classification).
%
%    [GP, DIAGNOSIS] = SVIGP(GP, X, Y, OPTIONS) also returns values
%    monitored during the iteration. The structure DIAGNOSIS contains
%    the energy in the field e, the likelihood and covariance function
%    parameters in the field w, and the mean log predictive density (if xt
%    and yt is provided) in the field mlpd. The first dimension corresponds
%    to the main iteration in all these arrays. The second dimension
%    corresponds to the minibatch iteration in e and w. The third dimension
%    corresponds to the different parameters in w.
%
%    OPTIONS is optional parameter-value pair
%      xt               - test inputs. Used for monitoring the mean log
%                         predictive density, N.B. the monitoring needs
%                         both xt and yt.
%      yt               - observed yt in test points. Used for monitoring
%                         the mean log predictive density, N.B. the
%                         monitoring needs both xt and yt.
%      z                - optional observed quantity in triplet
%                         (x_i,y_i,z_i) Some likelihoods may use this. For
%                         example, in case of Poisson likelihood we have
%                         z_i=E_i, that is, expected value for ith case.
%      zt               - optional observed quantity in triplet
%                         (xt_i,yt_i,zt_i) Some likelihoods may use this.
%                         For example, in case of Poisson likelihood we
%                         have z_i=E_i, that is, the expected value for the
%                         ith case. N.B. used only in the monitoring of the
%                         mean log predictive density.
%      X_u              - inducing inputs. If omitted, kmeans clustering is
%                         applied and the resulting cluster centroids are
%                         selected. The number of inducing variables is
%                         then controlled by the parameter nu (see below).
%      nu               - the number of inducing variables if X_u is
%                         omitted. Can be given as an absolute value or
%                         relative to the input size. The default is
%                         min(floor(0.1*n),1500), where n is the number
%                         of training inputs.
%      m                - initial mean of the inducing variables. The
%                         default is zero vector.
%      S                - initial covariance of the inducing variables. The
%                         default is 0.1 times identity matrix.
%      n_minibatch      - absolute or relative size of the minibatches
%                         (relative to the number of the training inputs).
%                         Does not have to be divisible with the number of
%                         training inputs. The default is 0.1 (relative).
%      maxiter          - the maximum number of iterations (default 5000).
%                         Providing 0 initialises the gp structure
%                         parameters without optimisation.
%      momentum         - momentum term for the covariance function
%                         parameters (default 0.9)
%      mu1              - initial step size of the variational parameters
%                         (default 0.01).
%      mu2              - initial step size of the likelihood and
%                         covariance function parameters (default 1e-5)
%      tol              - tolerance of energy for the convergence
%                         (default 1e-6).
%      step_size        - function handle for the step size as a function
%                         of the iteration. The default function is f(i) =
%                         1/(1+i/n_minibatch), where n_minibatch is the
%                         size of the minibatches.
%      lik_sigma2       - likelihood variance in the case of non-gaussian
%                         likelihood (default 0.1).
%      lik_sigma2_prior - prior for the likelihood variance in the case of
%                         non-gaussian likelihood. The default is
%                         prior_loggaussian('mu',-2, 's2', 0.5).
%      display          - Control the amount of diagnostic verbosity.
%                         'off' displays nothing, 'final' display the
%                         final output, and 'iter' displays output at
%                         each iteration. Alternatively by providing a
%                         scalar value, fewer iterations can be displayed,
%                         e.g. value 10 displays only every tenth
%                         value (default behaviour).
%
%  See also
%    GP_SET, GPSVI_PRED, GPSVI_PREDGRAD, GPSVI_E, GPSVI_G, DEMO_SVI*
%
%  References:
%    Hensman, J., Fusi, N. and Lawrence, N. D. (2013). Gaussian processes
%    for big data. arXiv preprint arXiv:1309.6835.

% Copyright (c) 2014 Ville Tolvanen
% Copyright (c) 2014 Tuomas Sivula

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

ip=inputParser;
ip.FunctionName = 'SVIGP';
ip.addRequired('gp',@isstruct);
ip.addRequired('x', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
ip.addRequired('y', @(x) ~isempty(x) && isreal(x) && all(isfinite(x(:))))
ip.addParamValue('xt', [], @(x) isnumeric(x) && isreal(x) && all(isfinite(x(:))))
ip.addParamValue('yt', [], @(x) isnumeric(x) && isreal(x) && all(isfinite(x(:))))
ip.addParamValue('z', [], @(x) isnumeric(x) && isreal(x) && all(isfinite(x(:))))
ip.addParamValue('zt', [], @(x) isnumeric(x) && isreal(x) && all(isfinite(x(:))))
ip.addParamValue('X_u', [], @(x) isnumeric(x) && isreal(x) && all(isfinite(x(:))))
ip.addParamValue('nu', [], @(x) isreal(x) && isscalar(x) && x > 0)
ip.addParamValue('m', [], @(x) isnumeric(x) && isreal(x) && all(isfinite(x(:))))
ip.addParamValue('S', [], @(x) isnumeric(x) && isreal(x) && all(isfinite(x(:))))
ip.addParamValue('n_minibatch', 0.1, @(x) isreal(x) && isscalar(x) && x > 0)
ip.addParamValue('maxiter', 2000, @(x) isreal(x) && isscalar(x) && x >= 0)
ip.addParamValue('momentum', 0.9, @(x) isreal(x) && isscalar(x) && x > 0)
ip.addParamValue('mu1', 0.01, @(x) isreal(x) && isscalar(x) && x > 0)
ip.addParamValue('mu2', 1e-5, @(x) isreal(x) && isscalar(x) && x > 0)
ip.addParamValue('tol', 1e-6, @(x) isreal(x) && isscalar(x))
ip.addParamValue('step_size', [], @(x) isa(x,'function_handle'))
ip.addParamValue('lik_sigma2',0.1, @(x) isscalar(x) && x>=0);
ip.addParamValue('lik_sigma2_prior',prior_loggaussian('mu',-2, 's2', 0.5), ...
  @(x) isstruct(x) || isempty(x));
ip.addParamValue('display', 10, @(x) ismember(x,{'final','iter','off'}) ...
  || (isreal(x) && isscalar(x) && x > 1) )

ip.parse(gp, x,y,varargin{:});
x=ip.Results.x;
y=ip.Results.y;
z=ip.Results.z;
xt=ip.Results.xt;
yt=ip.Results.yt;
zt=ip.Results.zt;
X_u=ip.Results.X_u;
nu=ip.Results.nu;
m=ip.Results.m;
S=ip.Results.S;
n_minibatch=ip.Results.n_minibatch;
momentum=ip.Results.momentum;
mu1=ip.Results.mu1;
mu2=ip.Results.mu2;
step_size=ip.Results.step_size;
maxiter=ip.Results.maxiter;
tol=ip.Results.tol;
lik_sigma2 = ip.Results.lik_sigma2;
lik_sigma2_prior = ip.Results.lik_sigma2_prior;
display = ip.Results.display;

% Initialise the diagnosis output structure
diagnosis = struct();

% Check if latent method SVI has been set
if ~isfield(gp, 'latent_method') || ~isequal(gp.latent_method, 'SVI')
  gp=gp_set(gp, 'latent_method', 'SVI');
end

% Check if latent method is not gaussian or probit
if ~strcmp(gp.lik.type, 'Gaussian') && ~strcmp(gp.lik.type, 'Probit')
  error('Supported likelihoods for SVIGP are gaussian and probit.')
end

% Process parameters
if xor(isempty(xt), isempty(yt))
  warning('Need both xt and yt for monitoring mean log predictive density.');
end
n=size(x,1);

if n_minibatch < 1
  n_minibatch = max(floor(n_minibatch*n),1);
end
if n_minibatch > n
  n_minibatch = max(floor(0.1*n),1);
  warning('Too many minibatches, using floor(0.1*n) = %d instead.', ...
    n_minibatch)
end

if isempty(step_size)
  step_size=@(iter) 1/(1+iter./n_minibatch);
end

% Handle the inducing inputs
if ~ismember('X_u',ip.UsingDefaults)
  gp.X_u = X_u;
end
if isempty(gp.X_u)
  % Assign X_u by clustering
  if isempty(nu)
    nu = min(max(floor(0.1.*n),1),1500);
  elseif nu < 1
    nu = max(floor(nu.*n),1);
  end
  fprintf('Assign inducing inputs by clustering\n')
  Sw=warning('off','stats:kmeans:EmptyCluster');
  [~,X_u] = kmeans(x, nu,'Start','uniform',...
    'EmptyAction','singleton');
  warning(Sw);
  gp.X_u = X_u;
end
gp.nind = size(gp.X_u,1);

% Handle the rest of the parameters
if ~ismember('m',ip.UsingDefaults)
  if length(m) == gp.nind
    gp.m = m;
  else
    error('The size of m does not match with X_u')
  end
elseif ~isfield(gp, 'm') || length(gp.m) ~= gp.nind
  gp.m = zeros(gp.nind,1);
end

if ~ismember('S',ip.UsingDefaults)
  if ismatrix(S) && all(size(S) == gp.nind)
    gp.S = S;
  else
    error('The size of S does not match with X_u')
  end
elseif ~isfield(gp, 'S') || any(size(gp.S) ~= gp.nind)
  gp.S = 0.1*eye(gp.nind);
  % gp.S = gp_trcov(gp,gp.X_u);
end
gp.t1=gp.S\gp.m;
gp.t2=-0.5.*inv(gp.S);

% Handle the likelihood variance
if ~isfield(gp.lik, 'sigma2')
  gp.lik.sigma2 = lik_sigma2;
  gp.lik.p.sigma2 = lik_sigma2_prior;
  gp.lik.fh.lp = @lik_lp;
  gp.lik.fh.lpg = @lik_lpg;
end

% Return prematurely if only initialising
if maxiter == 0
  return
end

% Parameters
w = gp_pak(gp);
w0 =w;
nh1 = numel(gp.t1) + numel(gp.t2);
nh2 = length(w)-nh1;

% Initial step-size vector
mu0 = mu1.*ones(size(w));
mu0(end-nh2+1:end)=mu2;

% Size of minibatches
nb=n_minibatch;
% Number of minibatches
nbb=ceil(n/nb);

% Monitored values
if nargout > 1
  e_all = zeros(maxiter,nbb);
  w_all = zeros(maxiter,nbb,nh2);
  if ~isempty(xt) && ~isempty(yt)
    mlpd_all = zeros(maxiter,1);
    rmse_all = zeros(maxiter,1);
  end
end
g_old=zeros(size(w));
momentum=momentum.*ones(size(w));
etot_old=Inf;

% Preprocess conditions for iteration
disp_iter = strcmp(display, 'iter');
disp_i = 0;
disp_count = display;

% Iterate until convergence or maxiter
converged = 0;
try_fix_mu2 = 0;
for iter = 1:maxiter

  % Divide the data into minibatches
  inds = cell(nbb,1);
  ind = randperm(n);
  for i=1:nbb-1
    inds{i} = ind((i-1)*nb+1:i*nb);
  end
  inds{nbb} = ind((nbb-1)*nb+1:end);

  % Compute the step-size
  mu = mu0.*step_size(iter);

  % Iterate all the minibatches
  etot = 0;
  broken = 0;
  for i=1:nbb
    if isempty(z)
      zi = [];
    else
      zi = z(inds{i},:);
    end
    gp.data_prop=length(inds{i})./n;
    [e,~,~,param] = gpsvi_e(w,gp,x(inds{i},:),y(inds{i},:), 'z', zi);
    etot = etot+e;
    g = gpsvi_g(w,gp,x(inds{i},:),y(inds{i},:), 'z', zi, ...
      'gpsvi_e_param', param);
    g = mu.*g + momentum.*g_old;
    g_old = g;
    w = w+g;
    if nargout > 1
      e_all(iter,i) = e;
      w_all(iter,i,:) = w(end-nh2+1:end);
    end
    if ~isnan(etot) ...
        && all(~isinf(exp(w(end-nh2+1:end)))) ...
        && all(exp(w(end-nh2+1:end))~=0) ...
        && ~any(isnan(g)) ...
        && ~isnan(gpsvi_e(w+g,gp,x(inds{i},:),y(inds{i},:), 'z', zi))
      gp = gp_unpak(gp,w);
    elseif ~try_fix_mu2
      fprintf('Bad parameter values, decreasing mu2.\n');
      try_fix_mu2 = 1;
      mu2 = 0.1*mu2;
      mu0(end-nh2+1:end)=mu2;
      w = w0;
      g_old = zeros(1,nh1+nh2);
      broken = 1;
      break
    else
      fprintf('Bad parameter values, decreasing step-size and momentum.\n');
      mu0 = 0.1.*mu0;
      momentum = 0.5*momentum;
      w = w0;
      g_old = zeros(1,nh1+nh2);
      broken = 1;
      break
    end
    gpsvi_e('clearcache',gp);
  end
  if broken
    continue
  end
  etot=etot/nbb;

  % Analyse and print
  if isscalar(display)
    if disp_count == display
      disp_i = 1;
      disp_count = 1;
    else
      disp_i = 0;
      disp_count = disp_count +1;
    end
  end
  if ~isempty(xt) && ~isempty(yt) ...
      && ( disp_iter || nargout > 1 || disp_i)
    [Eft,~,lpyt] = gpsvi_pred(gp,x,y,xt,'yt',yt, 'z', zi, 'zt', zt);
    lpyt = mean(lpyt);
    if nargout > 1
      mlpd_all(iter) = lpyt;
      rmse = sqrt(mean((yt-Eft).^2));
      rmse_all(iter) = rmse;
    end
    if disp_iter || disp_i
      fprintf('iter=%d/%d, e=%.3f, mlpd=%.4g, rmse=%.4g, de=%.5g\n', ...
        iter, maxiter, etot, lpyt, rmse, abs(etot-etot_old));
    end
  elseif disp_iter || disp_i
    fprintf('iter=%d/%d, e=%.3f, de=%.5g\n', ...
      iter, maxiter, etot, abs(etot-etot_old));
  end

  % Check for convergence
  if abs(etot-etot_old)<tol
    converged = 1;
    if strcmp(display, 'iter')
      fprintf('Energy converged\n')
    end
    break
  end
  etot_old=etot;

end

% Display results
if ~converged
  fprintf('Iteration limit %d reached while optimising the parameters\n', ...
    maxiter)
elseif ~strcmp(display, 'off')
  fprintf('Tolerance reached in %d iterations\n', iter)
end
if strcmp(display, 'final') || isscalar(display)
  if ~isempty(xt) && ~isempty(yt) && ( nargout > 1 || disp_i)
    fprintf(['Final values:\n' ...
      'e=%.3f, mlpd=%.4g, rmse=%.4g\n'], etot, lpyt, rmse);
  elseif ~isempty(xt) && ~isempty(yt)
    [Eft,~,lpyt] = gpsvi_pred(gp,x,y,xt,'yt',yt, 'z', z, 'zt', zt);
    fprintf(['Final values:\n' ...
      'e=%.3f, mlpd=%.4g, rmse=%.4g\n'], ...
      etot, mean(lpyt)), sqrt(mean((yt-Eft).^2));
  else
    fprintf('Final energy: %.3f\n', etot);
  end
end

% Save the monitored values
if nargout > 1
  diagnosis.e = e_all(1:iter,:);
  diagnosis.w = w_all(1:iter,:,:);
  if ~isempty(xt) && ~isempty(yt)
    diagnosis.mlpd = mlpd_all(1:iter);
    diagnosis.rmse = rmse_all(1:iter);
  end
end

end

function lp = lik_lp(lik, varargin)
%LIK_LP  log(prior) of the likelihood parameters
%
%  Description
%    LP = LIK_LP(LIK) takes a likelihood structure LIK and
%    returns log(p(th)), where th collects the parameters. This
%    subfunction is needed if there are likelihood parameters.
%    Added for non-gaussian likelihoods in SVIGP.
%
%  See also
%    LIK_*, SVIGP

% If prior for sigma2sion parameter, add its contribution
lp=0;
if ~isempty(lik.p.sigma2)
  lp = lik.p.sigma2.fh.lp(lik.sigma2, lik.p.sigma2) +log(lik.sigma2);
end
end

function lpg = lik_lpg(lik)
%LIK_LPG  d log(prior)/dth of the likelihood parameters
%
%  Description
%    E = LIK_NEGBIN_LPG(LIK) takes a likelihood structure LIK and
%    returns d log(p(th))/dth, where th collects the parameters.
%    This subfunction is needed if there are likelihood parameters.
%    Added for non-gaussian likelihoods in SVIGP.
%
%  See also
%    LIK_*, SVIGP

lpg=[];
if ~isempty(lik.p.sigma2)
  % Evaluate the gprior with respect to sigma2
  ggs = lik.p.sigma2.fh.lpg(lik.sigma2, lik.p.sigma2);
  lpg = ggs(1).*lik.sigma2 + 1;
  if length(ggs) > 1
    lpg = [lpg ggs(2:end)];
  end
end
end


function [imageSeg]= getPlanSeg_complete(XYZworldframeTest,SpaceTest,imgSize,imageNum)
% input XYZworldframeTest is alinged in Z
onPlaneThreshold =0.055;
sizethr =750;
normalAgreeThreshold =0.8;

removeNaN = find(~sum(isnan(XYZworldframeTest),2));
XYZworldframeTest = XYZworldframeTest(removeNaN,:);
[zhist,zc]=hist(XYZworldframeTest(:,3),round(20*(SpaceTest.Rz(2)-SpaceTest.Rz(1))));
[~,zpks] =findpeaks([0,zhist,0],'minpeakdistance',3);
zpks = zpks-1;
% figure,
% bar(bincenter,count);
% hold on;
% plot(bincenter(pks),count(pks),'rx');
% remove
range =[max(min(XYZworldframeTest(:,3)),zc(max(1,zpks-1))'),...
        min(zc(min(size(zc,2),zpks+1)),max(XYZworldframeTest(:,3)))';
        zc(end-2),max(XYZworldframeTest(:,3));
        min(XYZworldframeTest(:,3)),zc(3)];
removepts = inrange(XYZworldframeTest(:,3),range);
XYZworldframeTestRemove=XYZworldframeTest;
XYZworldframeTestRemove(removepts>0,:) =[];

% hough lines
[H,T,R] = votelines(XYZworldframeTestRemove(:,1),XYZworldframeTestRemove(:,2));
% blur it
allangleVote = max(H);
allangleVote = [allangleVote,allangleVote];
shift= 0;
clear rightAngle
for i =1:length(shift)
    rightAngle(i,:) = allangleVote(91+shift(i):180+shift(i))+allangleVote(1:90);
end
[~,AnglepickedLin]= max(rightAngle(:));
[s,Anglepicked]=ind2sub(size(rightAngle),AnglepickedLin);
[~,Rpicked1] = max(H(:,Anglepicked));
[~,Rpicked2] = max(H(:,Anglepicked+90+shift(s)));
P = [R(Rpicked1),T(Anglepicked);R(Rpicked2),T(Anglepicked)+90+shift(s)];

rotation = min(P(:,2));
Rot = getRotationMatrix('z',-1*rotation/180*pi);
XYZworldframeTestNew = [[Rot(1:2,1:2)*XYZworldframeTest(:,[1,2])']', XYZworldframeTest(:,3)];
normals = points2normals(XYZworldframeTestNew);

% find  corner
[xhist,xc]=hist(XYZworldframeTestNew(:,1),round((SpaceTest.Rx(2)-SpaceTest.Rx(1))/0.05));
[~,xpks] =findpeaks([0,xhist,0],'MINPEAKHEIGHT',50);
xpks = xpks-1;
[yhist,yc]=hist(XYZworldframeTestNew(:,2),round((SpaceTest.Rx(2)-SpaceTest.Rx(1))/0.05));
[~,ypks] =findpeaks([0,yhist,0],'MINPEAKHEIGHT',50);
ypks = ypks-1;
[zhist,zc]=hist(XYZworldframeTestNew(:,3),round((SpaceTest.Rx(2)-SpaceTest.Rx(1))/0.05));
[~,zpks] =findpeaks([0,zhist,0],'MINPEAKHEIGHT',50);
zpks = zpks-1;

%% project points onto this plan and caculate conected component
gid =1;
Segment ={};
SegmentSize = [];
imageSegFill = zeros(imgSize);
for p =1:size(xpks,2)
    inLine =find(abs(XYZworldframeTestNew(:,1)-xc(xpks(p)))<onPlaneThreshold&abs(normals(1,:)')>normalAgreeThreshold);
    conthisSegMask = zeros(imgSize);
    conthisSegMask(removeNaN(inLine)) =1;
    label = bwlabel(conthisSegMask,8);
    unique_label = unique(label);
    unique_label(unique_label==0)=[];
    for i =1:length(unique_label),
         if sum(label(:)==unique_label(i))>sizethr,
            Segment(gid) = {label==unique_label(i)};
            SegmentSize(gid) = sum(label(:)==unique_label(i));
            imageSegFill(label==unique_label(i))=gid;
            gid = gid+1;
         end
    end
end

for p =1:size(ypks,2)
    inLine =find(abs(XYZworldframeTestNew(:,2)-yc(ypks(p)))<onPlaneThreshold&abs(normals(2,:)')>normalAgreeThreshold);
    conthisSegMask = zeros(imgSize);
    conthisSegMask(removeNaN(inLine)) =1;
    label = bwlabel(conthisSegMask,8);
    unique_label = unique(label);
    unique_label(unique_label==0)=[];
    for i =1:length(unique_label),
         if sum(label(:)==unique_label(i))>sizethr,

            Segment(gid) = {label==unique_label(i)};
            SegmentSize(gid) = sum(label(:)==unique_label(i));
            imageSegFill(label==unique_label(i))=gid;
            gid = gid+1;
         end
    end
end

floorSize = 0;
floorMask = zeros(imgSize);
Lowest = nanmin(XYZworldframeTestNew(:,3));
for p =1:size(zpks,2)
    inLine =find(abs(XYZworldframeTestNew(:,3)-zc(zpks(p)))<onPlaneThreshold&abs(normals(3,:)')>normalAgreeThreshold);
    conthisSegMask = zeros(imgSize);
    conthisSegMask(removeNaN(inLine)) =1;
    label = bwlabel(conthisSegMask,8);
    unique_label = unique(label);
    unique_label(unique_label==0)=[];
    for i =1:length(unique_label),
         if sum(label(:)==unique_label(i))>sizethr,
            Segment(gid) = {label==unique_label(i)};
            SegmentSize(gid) = sum(label(:)==unique_label(i));
            if p<2&&SegmentSize(gid)>floorSize&&zc(zpks(p))-Lowest<0.25,
                floorSize = SegmentSize(gid);
                floorMask = label==unique_label(i);
            end
            imageSegFill(label==unique_label(i))=gid;
            gid = gid+1;
         end
    end
end
%%

[SegmentSize,segid] = sort(SegmentSize,'descend');
Segment = Segment(segid);
cnt =1;
imageSeg = zeros(imgSize);
for i =1:length(Segment)
    idx = Segment{i}&imageSeg==0;
    se = strel('disk',3);
    idx2 = imclose(idx,se);
    idx(imageSegFill==0&(idx2&~idx))=1;
    if sum(idx(:))>sizethr
       imageSeg(idx)=cnt;
       cnt =cnt+1;
    end
end

while true,
    % remaining points
    remaining_points =imageSeg==0;
    cntPre = cnt;
    label = bwlabel(remaining_points,8);
    unique_label = unique(label);
    unique_label(unique_label==0)=[];
    directions = icosahedron2sphere(0);
    directions = directions(directions(:,2) <= 0,:);
    for i =1:length(unique_label)
        remaining_points_conn = label==unique_label(i);
        remaining_points_conn_ptsidx = find(remaining_points_conn(removeNaN));
        XYZworldframeTestRemaining_conn = XYZworldframeTest(remaining_points_conn_ptsidx,:);
        normals_conn = normals(:,remaining_points_conn_ptsidx);
        if size(XYZworldframeTestRemaining_conn,1)>sizethr
            [~, idx] = max(abs(directions * normals_conn),[],1);
            majorDirIdx = mode(idx);
            normalAgreeId = find(idx ==majorDirIdx);
            [B,~,inliers] = ransacfitplane(XYZworldframeTestRemaining_conn(normalAgreeId,:)', onPlaneThreshold);
            if sum(inliers)>sizethr
                thisSeg = zeros(imgSize);
                thisSeg(removeNaN(remaining_points_conn_ptsidx(normalAgreeId(inliers)))) =1;
                thisSeg_label = bwlabel(thisSeg,8);
                thisSeg_unique_label = unique(label);
                thisSeg_unique_label(thisSeg_unique_label==0)=[];
                for j = 1:length(thisSeg_unique_label),
                    if sum(thisSeg_label(:)==thisSeg_unique_label(j))>sizethr,
                        idxSeg = thisSeg_label==thisSeg_unique_label(j);
                        idxSeg = imclose(idxSeg,se);
                        imageSeg(idxSeg) =cnt;
                        cnt=cnt+1;
                    end
                end

            end
        end
    end
    if cntPre==cnt,
        break;
    end
end
map= [0,randperm(max(imageSeg(:)+1))];
map(map==1) =[];
imageSeg = map(imageSeg+1);
if sum(floorMask(:))>0,
    floorMask = imclose(floorMask,se);
    imageSeg(floorMask) = 1;
end
figure(1),imagesc(imageSeg);
figure(2),imagesc(floorMask);
%Boudary = [D1 D2 D3 D4];
if imageNum> 0,
    im = getImagesc(imageSeg);
    mkdir(segpath);
    imwrite(im,sprintf('%s/%04d.jpg',segpath,imageNum));

    im = getImagesc(floorMask);
    imwrite(im,sprintf('%s/%04d_floor.jpg',segpath,imageNum));

    save(sprintf('%s/%04d.mat',segpath,imageNum),'imageSeg','floorMask')

end
end

function [hough_transform,T,R] = votelines(X,Y)
         thetaResolution = 1;
         rhoResolution = 0.1;
         T = [1:180];
         theta = T/180*pi;
         rho = X(:)*cos(theta)+ Y(:)*sin(theta);
         % quantize the rho
         rhoNorm = max(1,round((rho-min(rho(:)))/rhoResolution));
         R =[1:max(rhoNorm(:))]*rhoResolution+min(rho(:));
         hough_transform = zeros(max(rhoNorm(:)),size(T,2));
         TT = repmat(T,[size(rhoNorm,1),1]);
         hough_transform = accumarray([rhoNorm(:),TT(:)],1,size(hough_transform));
end


function X=MergeMargCop(W,F,U)

[J,K]=size(W);
X=0*U;
for k=1:K
    dd = interp1(F(:,k),W(:,k),U(:,k),'linear','extrap');
    X(:,k) = dd;
end


function val = get_params(CPD, name)
% GET_PARAMS Get the parameters (fields) for a softmax_CPD object
% val = get_params(CPD, name)
%
% The following fields can be accessed
%
% weights - W(X,Y,Q)
% offset  - b(Y,Q)
%
% e.g., W = get_params(CPD, 'weights')

[W, b] = extract_params(CPD);
switch name
 case 'weights',   val = W;
 case 'offset',    val = b;
 otherwise,
  error(['invalid argument name ' name]);
end


function score = Min_value(Population,~)
% <min> <single> <real/integer/label/binary/permutation> <large/none> <constrained/none> <expensive/none> <sparse/none> <dynamic/none>
% The minimum objective value

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

	score = Population.best.objs;
    if isempty(score)
        score = nan;
    end
end


function timeplot(axs_hand,mode)
%
% Utility Function: TIMEPLOT
%
% The purpose of this function is to handle the plotting of all the time
% response options

% Author: Craig Borghesani
% Date: 8/8/94
% Revised: 11/15/94
% Copyright (c) 1999, Prentice-Hall

% if no time response axis present, get out
if ~length(axs_hand), return; end

% obtain handle information
f = gcf;
ui_data = get(f,'userdata');
ui_han = ui_data{1};
open_loop = ui_han(78);
clos_loop = ui_han(79);
unit_imp  = ui_han(80);
unit_step = ui_han(81);
unit_ramp = ui_han(82);
rise_time = ui_han(83);
dlay_time = ui_han(84);
peak_time = ui_han(85);
over_shot = ui_han(86);
setl_2per = ui_han(87);
setl_5per = ui_han(88);
cur_sys = get(ui_han(30),'userdata');
stat_bar = get(ui_han(43),'userdata');
range1 = 78:82;
range2 = 83:88;

% distribute handles
axs_data = get(axs_hand,'userdata');

if ~length(mode) | any(mode == [13,14,15]),

% determine various states of time response environment

 if ~length(mode), mode = 0; end
 ct = 2;
 for k = range1,
  if strcmp(get(ui_han(k),'checked'),'on'),
   mode(ct) = k-(range1(1)-1);
  else
   mode(ct) = 0;
  end
  ct = ct + 1;
 end

end

if length(mode) > 1,
 set(axs_data(2:length(axs_data)),'vis','off');
end

open_imp = axs_data(2);
clos_imp = axs_data(5);
open_step = axs_data(3);
clos_step = axs_data(6);
open_ramp = axs_data(4);
clos_ramp = axs_data(7);
tim_txt = axs_data(8:length(axs_data));

plant_mat = get(ui_han(3+cur_sys),'userdata');
for_mat  = get(ui_han(6+cur_sys),'userdata');
bac_mat  = get(ui_han(9+cur_sys),'userdata');

term_mat  = termjoin(plant_mat,for_mat,bac_mat);
[num,den] = termextr(term_mat);

if length(num)==1 & length(den) == 1,
 set(stat_bar,'string','No Time Response computed');
 return;
end

ln = length(num); ld = length(den);
if ln > ld,
 set(stat_bar,'string','You have more Zeros than Poles.  Time response not plotted.');
 return
end

if polyval(den,0) ~= 0,
 final = polyval(num,0)/polyval(den,0);
else
 final = inf;
end

[nump,denp] = termextr(plant_mat);
[numg,deng] = termextr(for_mat);
[numh,denh] = termextr(bac_mat);
numt = conv(conv(nump,numg),numh);
dent = conv(conv(denp,deng),denh);
lnt = length(numt); ldt = length(dent);
numt = [zeros(1,ldt-lnt),numt];
dent = [zeros(1,lnt-ldt),dent];
numcl = conv(conv(nump,numg),denh);
dencl = dent + numt;

finalcl = polyval(numcl,0)/polyval(dencl,0);

tvec = get(ui_han(40),'userdata');
imp_input = [1/(tvec(2)-tvec(1));zeros(length(tvec)-1,1)];
stp_input = ones(length(tvec),1);
rmp_input = tvec';

if term_mat(1,2)~=0, % pure delay time
 loc_t = find(tvec <= term_mat(1,2));
 len = length(loc_t);
 if len > 1,
  imp_input = [zeros(len,1);imp_input(1:(length(imp_input)-len))];
  stp_input = [zeros(len,1);stp_input(1:(length(stp_input)-len))];
  rmp_input = [zeros(len,1);rmp_input(1:(length(rmp_input)-len))];
 end
end

if any(mode==1), % Open-Loop

 if strcmp(get(open_loop,'checked'),'off') | length(mode)>1,
  set(open_loop,'checked','on');

  if strcmp(get(unit_imp,'checked'),'on'),

   set(stat_bar,'string','Computing open-loop impulse response');
   imp_ol = lsim(num,den,imp_input,tvec);
   set(open_imp,'xdata',tvec,'ydata',imp_ol,'vis','on');
   if length(mode) > 1, mode(4) = 0; end

  end

  if strcmp(get(unit_step,'checked'),'on'),

   set(stat_bar,'string','Computing open-loop step response');
   step_ol = lsim(num,den,stp_input,tvec);
   set(open_step,'xdata',tvec,'ydata',step_ol,'vis','on');
   if length(mode) > 1, mode(5) = 0; end

  end

  if strcmp(get(unit_ramp,'checked'),'on'),

   set(stat_bar,'string','Computing open-loop ramp response');
   ramp_ol = lsim(num,den,rmp_input,tvec);
   set(open_ramp,'xdata',tvec,'ydata',ramp_ol,'vis','on');
   if length(mode) > 1, mode(6) = 0; end

  end

 else

  set(stat_bar,'string','Removing Open-Loop');
  set(open_loop,'checked','off');
  set([open_imp,open_step,open_ramp,tim_txt([1:2:9])],'vis','off');

 end

end

if any(mode==2), % Closed-Loop

 if strcmp(get(clos_loop,'checked'),'off') | length(mode)>1,
  set(clos_loop,'checked','on');

  if strcmp(get(unit_imp,'checked'),'on'),

   set(stat_bar,'string','Computing closed-loop impulse response');
   imp_cl = lsim(numcl,dencl,imp_input,tvec);
   set(clos_imp,'xdata',tvec,'ydata',imp_cl,'vis','on');
   if length(mode) > 1, mode(4) = 0; end

  end

  if strcmp(get(unit_step,'checked'),'on'),

   set(stat_bar,'string','Computing closed-loop step response');
   step_cl = lsim(numcl,dencl,stp_input,tvec);
   set(clos_step,'xdata',tvec,'ydata',step_cl,'vis','on');
   if length(mode) > 1, mode(5) = 0; end

  end

  if strcmp(get(unit_ramp,'checked'),'on'),

   set(stat_bar,'string','Computing closed-loop ramp response');
   ramp_cl = lsim(numcl,dencl,rmp_input,tvec);
   set(clos_ramp,'xdata',tvec,'ydata',ramp_cl,'vis','on');
   if length(mode) > 1, mode(6) = 0; end

  end

 else

  set(stat_bar,'string','Removing Closed-Loop');
  set(clos_loop,'checked','off');
  set([clos_imp,clos_step,clos_ramp,tim_txt([2:2:10])],'vis','off');

 end

end

if any(mode==3), % unit impulse

 if strcmp(get(unit_imp,'checked'),'off') | length(mode)>1,
  set(unit_imp,'checked','on');

  if strcmp(get(open_loop,'checked'),'on'),

   set(stat_bar,'string','Computing open-loop impulse response');
   imp_ol = lsim(num,den,imp_input,tvec);
   set(open_imp,'xdata',tvec,'ydata',imp_ol,'vis','on');

  end

  if strcmp(get(clos_loop,'checked'),'on'),

   set(stat_bar,'string','Computing closed-loop impulse response');
   imp_cl = lsim(numcl,dencl,imp_input,tvec);
   set(clos_imp,'xdata',tvec,'ydata',imp_cl,'vis','on');

  end

 else

  set(stat_bar,'string','Removing impulse response');
  set(unit_imp,'checked','off');
  set([open_imp,clos_imp],'vis','off');

 end

end

if any(mode==4), % unit step

 if strcmp(get(unit_step,'checked'),'off') | length(mode)>1,
  set(unit_step,'checked','on');

  if strcmp(get(open_loop,'checked'),'on'),

   set(stat_bar,'string','Computing open-loop step response');
   step_ol = lsim(num,den,stp_input,tvec);
   set(open_step,'xdata',tvec,'ydata',step_ol,'vis','on');

  end

  if strcmp(get(clos_loop,'checked'),'on'),

   set(stat_bar,'string','Computing closed-loop step response');
   step_cl = lsim(numcl,dencl,stp_input,tvec);
   set(clos_step,'xdata',tvec,'ydata',step_cl,'vis','on');

  end

  set(ui_han(range2),'enable','on');

 else

  set(stat_bar,'string','Removing step response');
  set(unit_step,'checked','off');
  set([open_step,clos_step,tim_txt],'vis','off');
  set(ui_han(range2),'enable','off');

 end

end

if any(mode==5), % unit ramp

 if strcmp(get(unit_ramp,'checked'),'off') | length(mode)>1,
  set(unit_ramp,'checked','on');

  if strcmp(get(open_loop,'checked'),'on'),

   set(stat_bar,'string','Computing open-loop ramp response');
   ramp_ol = lsim(num,den,rmp_input,tvec);
   set(open_ramp,'xdata',tvec,'ydata',ramp_ol,'vis','on');

  end

  if strcmp(get(clos_loop,'checked'),'on'),

   set(stat_bar,'string','Computing closed-loop ramp response');
   ramp_cl = lsim(numcl,dencl,rmp_input,tvec);
   set(clos_ramp,'xdata',tvec,'ydata',ramp_cl,'vis','on');

  end

 else

  set(stat_bar,'string','Removing ramp response');
  set(unit_ramp,'checked','off');
  set([open_ramp,clos_ramp],'vis','off');

 end

end

if any(mode < 6),

 pageview(1,axs_hand);
 ct = 1;
 mode = 0;
 for k = range2,
  if strcmp(get(ui_han(k),'checked'),'on'),
   mode(ct) = k-(range1(1)-1);
  else
   mode(ct) = 0;
  end
  ct = ct + 1;
 end

end

if any(mode==6) & strcmp(get(rise_time,'enable'),'on'), % rise time

 if strcmp(get(rise_time,'checked'),'off') | length(mode)>1,

  if strcmp(get(open_loop,'checked'),'on'),
   set(stat_bar,'string','Computing open-loop rise time');
   set(rise_time,'checked','on');
   amp = get(open_step,'ydata');
   diff_amp = diff(amp);
   loc_neg = find(diff_amp < 0);
   if ~length(loc_neg), % overdamped system
    less10 = find(amp>=0.1*final);
    less90 = find(amp>=0.9*final);
    riset = tvec(less90(1)) - tvec(less10(1));
    risea = amp(less90(1));
    set(tim_txt(1),'pos',[riset,risea,0],...
            'string',['Rise time (10-90%) = ',num2str(riset)],'vis','on');
   else  % underdamped system
    less0 = 1;
    less100 = find(amp>=final);
    riset = tvec(less100(1)) - tvec(less0);
    risea = amp(less100(1));
    set(tim_txt(1),'pos',[riset,risea,0],...
            'string',['Rise time (0-100%) = ',num2str(riset)],'vis','on');
   end
  end

  if strcmp(get(clos_loop,'checked'),'on'),
   set(stat_bar,'string','Computing closed-loop rise time');
   set(rise_time,'checked','on');
   amp = get(clos_step,'ydata');
   diff_amp = diff(amp);
   loc_neg = find(diff_amp < 0);
   if ~length(loc_neg), % overdamped system
    less10 = find(amp>=0.1*finalcl);
    less90 = find(amp>=0.9*finalcl);
    riset = tvec(less90(1)) - tvec(less10(1));
    risea = amp(less90(1));
    set(tim_txt(2),'pos',[riset,risea,0],...
          'string',['Rise time (10-90%) = ',num2str(riset)],'vis','on');
   else % underdamped system
    less0 = 1;
    less100 = find(amp>=finalcl);
    riset = tvec(less100(1)) - tvec(less0);
    risea = amp(less100(1));
    set(tim_txt(1),'pos',[riset,risea,0],...
            'string',['Rise time (0-100%) = ',num2str(riset)],'vis','on');
   end
  end

 else

  set(stat_bar,'string','Removing rise time');
  set(rise_time,'checked','off');
  set(tim_txt(1:2),'vis','off');
 end

end

if any(mode==7) & strcmp(get(dlay_time,'enable'),'on'), % delay time

 if strcmp(get(dlay_time,'checked'),'off') | length(mode)>1,

  if strcmp(get(open_loop,'checked'),'on'),
   set(stat_bar,'string','Computing open-loop delay time');
   set(dlay_time,'checked','on');
   amp = get(open_step,'ydata');
   less50 = find(amp>=0.5*final);
   dlayt = tvec(less50(1));
   dlaya = amp(less50(1));
   set(tim_txt(3),'pos',[dlayt,dlaya,0],...
         'string',['Delay time = ',num2str(dlayt)],'vis','on');
  end

  if strcmp(get(clos_loop,'checked'),'on'),
   set(stat_bar,'string','Computing closed-loop delay time');
   set(dlay_time,'checked','on');
   amp = get(clos_step,'ydata');
   less50 = find(amp>=0.5*finalcl);
   dlayt = tvec(less50(1));
   dlaya = amp(less50(1));
   set(tim_txt(4),'pos',[dlayt,dlaya,0],...
         'string',['Delay time = ',num2str(dlayt)],'vis','on');
  end

 else

  set(stat_bar,'string','Removing delay time');
  set(dlay_time,'checked','off');
  set(tim_txt(3:4),'vis','off');
 end

end

if any(mode==8) & strcmp(get(peak_time,'enable'),'on'), % peak time

 if strcmp(get(peak_time,'checked'),'off') | length(mode)>1,

  if strcmp(get(open_loop,'checked'),'on'),
   amp = get(open_step,'ydata');
   diff_amp = diff(amp);
   loc_neg = find(diff_amp < 0);
   if length(loc_neg),
    set(stat_bar,'string','Computing open-loop peak time');
    set(peak_time,'checked','on');
    peakt = tvec(loc_neg(1)-1);
    peaka = amp(loc_neg(1)-1);
    set(tim_txt(5),'pos',[peakt,peaka,0],...
         'string',['Peak time = ',num2str(peakt)],'vis','on');
   else
    set(stat_bar,'string','Unable to determine open-loop peak time');
   end
  end

  if strcmp(get(clos_loop,'checked'),'on'),
   amp = get(clos_step,'ydata');
   diff_amp = diff(amp);
   loc_neg = find(diff_amp < 0);
   if length(loc_neg),
    set(stat_bar,'string','Computing closed-loop peak time');
    set(peak_time,'checked','on');
    peakt = tvec(loc_neg(1)-1);
    peaka = amp(loc_neg(1)-1);
    set(tim_txt(6),'pos',[peakt,peaka,0],...
         'string',['Peak time = ',num2str(peakt)],'vis','on');
   else
    set(stat_bar,'string','Unable to determine closed-loop peak time');
   end
  end

 else

  set(stat_bar,'string','Removing peak time');
  set(peak_time,'checked','off');
  set(tim_txt(5:6),'vis','off');
 end

end

if any(mode==9) & strcmp(get(over_shot,'enable'),'on'), % percent overshoot

 if strcmp(get(over_shot,'checked'),'off') | length(mode)>1,

  if strcmp(get(open_loop,'checked'),'on'),
   set(stat_bar,'string','Computing open-loop % overshoot');
   set(over_shot,'checked','on');
   amp = get(open_step,'ydata');
   [max_amp,k] = max(amp);
   perover = 100*(max_amp-final)/final;
   set(tim_txt(7),'pos',[tvec(k),max_amp,0],...
       'string',[sprintf('%0.2f',perover),'% overshoot'],'vis','on');
  end

  if strcmp(get(clos_loop,'checked'),'on'),
   set(stat_bar,'string','Computing closed-loop % Overshoot');
   set(over_shot,'checked','on');
   amp = get(clos_step,'ydata');
   [max_amp,k] = max(amp);
   perover = 100*(max_amp-finalcl)/finalcl;
   set(tim_txt(8),'pos',[tvec(k),max_amp,0],...
       'string',[sprintf('%0.2f',perover),'% overshoot'],'vis','on');
  end

 else

  set(stat_bar,'string','Removing % overshoot');
  set(over_shot,'checked','off');
  set(tim_txt(7:8),'vis','off');
 end

end

if any(mode==10) & strcmp(get(setl_2per,'enable'),'on'), % 2% settling time

 if strcmp(get(setl_2per,'checked'),'off') | length(mode)>1,

  if strcmp(get(open_loop,'checked'),'on'),
   set(stat_bar,'string','Computing open-loop 2% settling time');
   set(setl_2per,'checked','on');
   amp = get(open_step,'ydata');
   per2 = length(amp);
   while (amp(per2)>0.98*final & amp(per2)<1.02*final), per2=per2-1; end
   per2t = tvec(per2);
   per2a = amp(per2);
   set(tim_txt(9),'pos',[per2t,per2a,0],...
         'string',['2%=',sprintf('%0.2f',per2t)],'vis','on');
  end

  if strcmp(get(clos_loop,'checked'),'on'),
   set(stat_bar,'string','Computing closed-loop 2% settling time');
   set(setl_2per,'checked','on');
   amp = get(clos_step,'ydata');
   per2 = length(amp);
   while (amp(per2)>0.98*finalcl & amp(per2)<1.02*finalcl), per2=per2-1; end
   per2t = tvec(per2);
   per2a = amp(per2);
   set(tim_txt(10),'pos',[per2t,per2a,0],...
         'string',['2%=',sprintf('%0.2f',per2t)],'vis','on');
  end

 else

  set(stat_bar,'string','Removing 2% settling time');
  set(setl_2per,'checked','off');
  set(tim_txt(9:10),'vis','off');
 end

end

if any(mode==11) & strcmp(get(setl_5per,'enable'),'on'), % 5% settling time

 if strcmp(get(setl_5per,'checked'),'off') | length(mode)>1,

  if strcmp(get(open_loop,'checked'),'on'),
   set(stat_bar,'string','Computing open-loop 5% settling time');
   set(setl_5per,'checked','on');
   amp = get(open_step,'ydata');
   per5 = length(amp);
   while (amp(per5)>0.95*final & amp(per5)<1.05*final), per5=per5-1; end
   per5t = tvec(per5);
   per5a = amp(per5);
   set(tim_txt(11),'pos',[per5t,per5a,0],...
         'string',['5%=',sprintf('%0.2f',per5t)],'vis','on');
  end

  if strcmp(get(clos_loop,'checked'),'on'),
   set(stat_bar,'string','Computing closed-loop 5% settling time');
   set(setl_5per,'checked','on');
   amp = get(clos_step,'ydata');
   per5 = length(amp);
   while (amp(per5)>0.95*finalcl & amp(per5)<1.05*finalcl), per5=per5-1; end
   per5t = tvec(per5);
   per5a = amp(per5);
   set(tim_txt(12),'pos',[per5t,per5a,0],...
         'string',['5%=',sprintf('%0.2f',per5t)],'vis','on');
  end

 else

  set(stat_bar,'string','Removing 5% settling time');
  set(setl_5per,'checked','off');
  set(tim_txt(11:12),'vis','off');
 end

end


function [] = advance_random()
%   Creates the next batch of 55 random numbers.

global oldrand ;

for j1 = 0:23
    new_random = oldrand(j1 + 1) - oldrand(j1 + 31 + 1);
    if (new_random < 0.0)
        new_random = new_random + 1.0 ;
    end
    oldrand(j1 + 1) = new_random ;
end

for j1 = 24:54
    new_random = oldrand(j1 + 1) - oldrand((j1 - 24) + 1);
    if (new_random < 0.0)
        new_random = new_random + 1.0 ;
    end
    oldrand(j1 + 1) = new_random ;
end
end


function center = initComponents_2p(obj, Y, K, debug_on, save_avi)
%% a greedy method for detecting ROIs and initializing CNMF. in each iteration,
% it searches the one with large (peak-median)/noise level and large local
% correlation
%% Input:
%   Y:  d X T matrx, imaging data
%   K:  scalar, maximum number of neurons to be detected.
%   debug_on: options for showing procedure of detecting neurons
%% Output:
%       Ain:  d X K' matrix, estimated spatial component
%       Cin:  K'X T matrix, estimated temporal component
%       bin:  d X nb matrix/vector, spatial components of the background
%       Cin:  nb X T matrix/vector, temporal components of the background
%       center: K' X 2, coordinate of each neuron's center
%       res:  d X T, residual after initializing Ain, Cin, bin, fin

%% Author: Pengcheng Zhou, Carnegie Mellon University.
% the method is an modification of greedyROI method used in Neuron paper of Eftychios
% Pnevmatikakis et.al. https://github.com/epnev/ca_source_extraction/blob/master/utilities/greedyROI2d.m
%% In each iteration of initializing neurons, it searchs the one with maximum
% value of (max-median)/noise * Cn, which selects pixels with high SNR and
% local correlation.

%% parameters
if exist('K', 'var')
    K = 200;
end
Y = obj.reshape(Y, 1);
Y_std = get_noise_fft(Y);
options = obj.options;

if ~exist('debug_on', 'var'); debug_on = false; end
if ~exist('save_avi', 'var'); save_avi=false; end
d1 = options.d1;
d2 = options.d2;
gSig = options.gSig;
gSiz = options.gSiz;
if and(isempty(gSiz), isempty(gSig)); gSig = 3; gSiz = 10; end
if isempty(gSiz); gSiz=3*gSig; end
if isempty(gSig); gSig=gSiz/3; end
if isfield(options, 'min_corr')
    min_corr = options.min_corr;    % minimum local correaltion value to start one neuron
else
    min_corr = 0.3;
end
if isfield(options, 'deconv_flag')
    deconv_flag = options.deconv_flag;
    deconv_options_0= options.deconv_options;
else
    deconv_flag = false;
end
if isfield(options, 'min_pnr')
    min_pnr = options.min_pnr;
else
    min_pnr = 2;
end
nb = options.nb;        % number of the background
pSiz = 1;       % after selecting one pixel, take the mean of square box
%near the pixel as temporal activity. the box size is (2*pSiz+1)
psf = ones(gSig)/(gSig^2);

maxIter = 5;            % iterations for refining results
sz = 4;            %distance of neighbouring pixels for computing local correlation

if ~ismatrix(Y); Y = reshape(Y, d1*d2, []); end;
[~, T] = size(Y);       % number of frames
Ain = zeros(d1*d2, K);  % spatial components
Cin = zeros(K, T);      % temporal components
Cin_raw = zeros(K, T);      % temporal components
Sin = zeros(K, T);      % temporal components
center = zeros(K, 2);   % center of the initialized components
kernel_pars = cell(K,1);    % parameters for the convolution kernels of all neurons

%% compute correlation image and (max-median)/std ratio
ind_frame = round(linspace(1, T, min(T, 1000)));    % select few frames for speed issues
%tmp_noise = randn(d1*d2, length(ind_frame));
C1 = correlation_image(full(Y(:, ind_frame)), sz, d1, d2);
Cb =  zeros(size(C1)); %correlation_image(full(Y(:, ind_frame(1:3:end)))+tmp_noise(:, 1:3:end), [gSiz, gSiz+1], d1, d2);  %backgroung correlatin
Cn = C1-Cb; %here Cb is the background correlation. for 2photon imaging results. It might be useful when the background signal is large
Y_median = median(Y(:, ind_frame), 2);
Y = bsxfun(@minus, Y, Y_median);
% Y_std = sqrt(mean(Y.*Y, 2));

%% find local maximum
k = 0;      %number of found components
min_pixel = floor(gSig^2/2);  % minimum number of peaks to be a neuron
peak_ratio = full(max(Y, [], 2))./Y_std; %(max-median)/std
peak_ratio(isinf(peak_ratio)) = 0;  % avoid constant values
peak_ratio(peak_ratio<min_pnr) = 0;

% save_avi = false;   %save procedures for demo
if debug_on
    figure('position', [100, 100, 800, 650]); %#ok<*UNRCH>
      figure('position', [100, 100, 1200, 800], 'color', [1,1,1]*0.9); %#ok<*UNRCH>
    set(gcf, 'defaultAxesFontSize', 20);
    ax_cn = axes('position', [0.04, 0.5, 0.3, 0.4]);
    ax_cn_varying = axes('position', [0.36, 0.5, 0.3, 0.4]);
    ax_cn_box = axes('position', [0.68, 0.54, 0.24, 0.32]);
    ax_raw = axes('position', [0.05, 0.25, 0.92, 0.2]);
    ax_trace = axes('position', [0.05, 0.01, 0.92, 0.2]);
    axes(ax_cn);
    imagesc(Cn);  hold on;
    axis equal off tight;
    title('correlation image');
    axes(ax_cn_varying);
    imagesc(Cn, [0,1]);
    axis equal off tight; hold on;
    if save_avi
        avi_file = VideoWriter('greedyROI_example.avi');
        avi_file.open();
    end
end

max_thresh = min_pnr * (min_corr);
while k<K
    %% find the pixel with the maximum ratio
    [max_v, ind_p] = max(peak_ratio.*(Cn(:)));
    peak_ratio(ind_p) = 0;  % no longer visit this pixel any more
    if max_v<max_thresh; break; end
    if Cn(ind_p)<min_corr; continue; end % ignore this local maximum due to small local correlation
    if  max_v/(min_corr)< min_pnr;     continue;    end

    [r, c] = ind2sub([d1,d2], ind_p);

    % select its neighbours for computing correlation
    rsub = max(1, -gSiz+r):min(d1, gSiz+r);
    csub = max(1, -gSiz+c):min(d2, gSiz+c);
    [cind, rind] = meshgrid(csub, rsub);
    nr = length(rsub);  %size of the neighboring matrix
    nc = length(csub);
    ind_nhood = sub2ind([d1, d2], rind(:), cind(:));
    Y_box = Y(ind_nhood, :);

    % draw a small area near the peak and extract the mean activities
    r0 = rsub(1); c0 = csub(1);
    rsub = (max(1, -pSiz+r):min(d1, pSiz+r)) - r0+1;
    csub = (max(1, -pSiz+c):min(d2, pSiz+c)) -c0+1;
    [cind, rind] = meshgrid(csub, rsub);
    ind_peak = sub2ind([nr, nc], rind(:), cind(:));
    y0 = mean(Y_box(ind_peak, :), 1);
%     y0(y0<0) = 0;

    % compute the correlation between the peak and its neighbours
    temp = reshape(corr(y0', Y_box'), nr, nc);
    active_pixel = full(temp>min_corr/2);
    l = bwlabel(active_pixel, 8);   % remove disconnected components
    active_pixel(l~=mode(l(ind_peak))) = false;
    tmp_v = sum(active_pixel(:));    %number of pixels with above-threshold correlation
    if debug_on
        axes(ax_cn_varying); cla;
        imagesc(reshape(Cn(:), d1, d2), [0, Cn(ind_p)]);
        title(sprintf('neuron %d', k+1));
        axis equal off tight; hold on;
        plot(c,r, 'om');
        axes(ax_cn_box);
        imagesc(temp, [min_corr, 1]);
        axis equal off tight;
        title('local corr.');
        axes(ax_raw); cla; hold on;
        plot(y0/max(y0)); title('activity in the center');
        axis off tight;
        if ~save_avi; pause; end
    end
    if tmp_v<min_pixel;         continue;  end % neuron is too small

    %% save neuron
    %   nonzero area
    data = Y_box(active_pixel(:), :);
    ind_active = ind_nhood(active_pixel(:));  %indices of active pixels within the whole frame
    peak_ratio(ind_nhood(ind_peak)) = 0;    % the small area near the peak is not able to initialize neuron anymore

    % do a rank-1 matrix factorization in this small area
    [ai, ci_raw] = finetune2d(data, y0, maxIter);
    if norm(ai)==0;        continue;   end
    k = k+1;
    if deconv_flag
        % deconv the temporal trace
        [ci, si, deconv_options] = deconvolveCa(ci_raw, deconv_options_0);  % sn is 1 because i normalized c_raw already
        % save this initialization
        Ain(ind_active, k) = ai;
        Cin(k, :) = ci;
        Sin(k, :) = si;
        Cin_raw(k, :) = ci_raw;
        %                 kernel_pars(k, :) = kernel.pars;
        kernel_pars{k} = reshape(deconv_options.pars, 1, []);
    else
        ci = ci_raw;
        Ain(ind_active, k) = ai;
        Cin(k, :) = ci_raw;
        Cin_raw(k, :) = ci_raw;
    end
    ci = reshape(ci, 1, []);
    Y(ind_active, :) = data-ai*ci;
    center(k, :) = [r, c];

    if debug_on
        axes(ax_cn);
        plot(c, r, '.r');
        axes(ax_cn_varying);
        plot(c,r, 'or');
        axes(ax_cn_box);
        temp = zeros(nr, nc); temp(active_pixel) = ai;
        imagesc(temp);
        axis equal off tight;
        title('spatial component');
        axes(ax_trace);  cla; hold on;
        if deconv_flag
                    plot(ci_raw, 'linewidth', 2); title('temporal component'); axis tight;
            plot(ci, 'r', 'linewidth', 1);  axis tight;
            legend('raw trace', 'denoised trace');
        else
            plot(ci_raw, 'r', 'linewidth', 1); title('temporal component'); axis tight;
        end
        title('temporal component');
        if exist('avi_file', 'var')
            temp = getframe(gcf);
            temp.cdata = imresize(temp.cdata, [800, 640]);
            avi_file.writeVideo(temp);
        elseif ~save_avi
            temp = input('type s to stop the debug mode:  ', 's');
            if strcmpi(temp, 's')
                save_avi = true;
            end
        else
            drawnow();
        end
    end

    if mod(k, 10)==0
        fprintf('%d/%d neurons have been detected\n', k, K);
    end

    if k==K;   break; end

    %% udpate peak_ratio and correlation image
    tmp_old = peak_ratio(ind_active);
    tmp_new = max(Y(ind_active, :), [], 2)./Y_std(ind_active);
    temp = zeros(nr, nc);
    temp(active_pixel) = max(0, tmp_old-tmp_new); % after each iteration, the peak ratio can not be increased
    peak_ratio(ind_nhood) = max(0, peak_ratio(ind_nhood) - reshape(imfilter(temp, psf), [], 1)); % update peak_ratio, results are smoothed
    Cn(ind_nhood) = correlation_image(full(Y(ind_nhood, ind_frame)), sz, nr, nc)-reshape(Cb(ind_nhood), nr, nc);  % update local correlation
end

center = center(1:k, :);
obj.A = sparse(Ain(:, 1:k));
obj.C = Cin(1:k, :);
obj.C_raw = Cin_raw(1:k, :);
obj.S = Sin(1:k, :);
obj.P.kernel_pars = kernel_pars(1:k);

if exist('avi_file', 'var'); avi_file.close(); end
res = bsxfun(@plus, Y, Y_median);

% %% initialize background
% tsub = max(1, round(T/1000));
% [bin, f] = nnmf(max(res(:, 1:tsub:T), 0), nb);
% fin = imresize(f, [nb, T]);
% fin = HALS_temporal(max(res, 0), bin, fin, maxIter);
% obj.b = HALS_spatial(max(res, 0), bin, fin, [], maxIter);
% obj.f = fin;
end

function [ai, ci] = finetune2d(data, ci, nIter)
%do matrix factorization given the model data = ai*ci, where ai>=0
%
%Input:
%   data:   d x T matrix, small patch containing one neuron
%   ci:     initial value for trace
%   nIter  number of coordinate descent steps
%
%Output:
%   ai  M x N matrix, result of the fine-tuned neuron shape
%   ci  1 x T matrix, result of the neuron
%% copied from greedyROI.m

if ~exist('nIter', 'var'), nIter = 1; end
data(data<0)= 0;
%do block coordinate descent
for iter = 1:nIter,
    %update basis
    ai = max(0, (data*ci')/(ci*ci'));
    norm_ai = norm(ai, 2);
    if norm_ai==0; break;     end
    ai = ai/norm_ai;
    ci =  (ai'*data);
    %     ci(ci<0) = 0;
end
% [b, sn] = estimate_baseline_noise(ci);
% ci = ci - b;
% ai = ai*sn;
% ci = ci/sn;
end


function [newROIcoords,tmpNewROIcoords] = xformROIcoords(ROIcoords,Xform,inputVoxSize,outputVoxSize,sampRate)
%
% newROIcoords = xformROIcoords(ROIcoords,Xform,inputVoxSize,outputVoxSize,[sampRate])
%
% Transforms ROI coords using Xform, supersampling in each
% dimension to accumulate partial volumes, then keeping only
% those voxels with partial volumes above thresh.
%
% ROIcoords: 3xN matrix of coordinates (y,x,z).
% Xform: 4x4 homogeneous transform
% inputVoxSize: 3-vector, size of voxels (mm) in ROIcoords
% outputVoxSize: 3-vector, size of voxels (mm) in newROIcoords
% sampRate: 3-vector, supersampling rate for each dimension
%           default is odd number >= 4x ratio of inputVoxSize/outputVoxSize
%
% newROIcoords: 3xN matrix of (y,x,z)
%
% djh, 8/98.  Modified from ROIcoords/transformROI.m in mrLoadRet-1
% 7/19/02 djh, modified to maintain equal volumes
% 11/02/05 ARW now returns the 'raw' transformed coordinates - useful for
% various applications including interfacing with SPM
if ~isa(ROIcoords, 'double'), ROIcoords = double(ROIcoords); end

if ~exist('sampRate','var')
	sampRate = ceil(inputVoxSize ./ outputVoxSize) .* [4,4,4];
	sampRate = 2*floor(sampRate/2) + 1;
end

% Convert ROI coords to homogenous coordinates, by adding a fourth
% row of 1's, and transform.
%
tmpNewROIcoords = ones(4,size(ROIcoords,2));
tmpNewROIcoords(1:3,:) = ROIcoords;
tmpNewROIcoords = Xform * tmpNewROIcoords;

% Find bounding (min and max) volume.
%
minPos = [min(tmpNewROIcoords(1,:));min(tmpNewROIcoords(2,:));min(tmpNewROIcoords(3,:));1];
maxPos = [max(tmpNewROIcoords(1,:));max(tmpNewROIcoords(2,:));max(tmpNewROIcoords(3,:));1];
minPos = floor(minPos)-[2,2,2,0]';
maxPos = ceil(maxPos)+[2,2,2,0]';
dims = maxPos(1:3)-minPos(1:3)+ones(3,1);

% Initialize accumulator for partial volume calculation, a vector
% of length appropriate to index the bounding volume.
%
accum = zeros(1,prod(dims));

% Calculate offsets that will be added within the loop to do the
% partial voluming.
%
xoffsets=[-.5+1/(2*sampRate(1)):1/sampRate(1):.5-1/(2*sampRate(1))];
yoffsets=[-.5+1/(2*sampRate(2)):1/sampRate(2):.5-1/(2*sampRate(2))];
zoffsets=[-.5+1/(2*sampRate(3)):1/sampRate(3):.5-1/(2*sampRate(3))];
% xoffsets=[0:1/sampRate(1):1-1/sampRate(1)];
% yoffsets=[0:1/sampRate(2):1-1/sampRate(2)];
% zoffsets=[0:1/sampRate(3):1-1/sampRate(3)];

% Divide alpha by prod(sampRate) to get partial volume for the
% supersampled voxels.
%
alpha = repmat(1/prod(sampRate),[1 size(ROIcoords,2)]);

% Set up a mrvWaitbar if needed.
verbose = prefsVerboseCheck;
if verbose > 1
	waitHandle = mrvWaitbar(0,'Transforming ROI coordinates.  Please wait...');
end

% Loop through supersamples, transform them, and accumulate
% partial volume.
%
ROIcoords = double(ROIcoords);
for ioff=1:length(xoffsets)
	xoff=xoffsets(ioff);

	if verbose>1, mrvWaitbar(ioff/length(xoffsets)); end

	for yoff=yoffsets
		for zoff=zoffsets
			% Add offset
			tmpNewROIcoords(1:3,:) = ROIcoords + ...
				repmat([xoff;yoff;zoff],[1,size(ROIcoords,2)]);

			% Transform
			tmpNewROIcoords = Xform * tmpNewROIcoords;

			% Round and subtract minPos
			coords = round(tmpNewROIcoords(1:3,:)) - ...
				repmat(minPos(1:3),[1,size(tmpNewROIcoords,2)]);

			% Convert to indices and remove duplicates
			indices = coords2Indices(coords,dims);

			% Accumulate partial volume.  Need to do it in a loop
			% instead of:
			%    accum(indices) = accum(indices) + alpha;
			% because an index can appear twice in indices and we want
			% to accumulate them both.
			for jj=1:length(indices)
				accum(indices(jj)) = accum(indices(jj)) + alpha(jj);
			end
		end
	end
end

if verbose>1, close(waitHandle); end

% Build newROIcoords
%
[sortedAccum,indices] = sort(accum);
nonZeroSize = length(find(accum > 0));
newROIsize = round(prod(inputVoxSize)*size(ROIcoords,2) / prod(outputVoxSize));
newROIsize = max(1,newROIsize); % we don't want an ROI size of 0. This will cause an error.
newROIsize = min(nonZeroSize,newROIsize);
indices = indices(length(indices)-newROIsize+1:length(indices));
if ~isempty(indices)
  newROIcoords = indices2Coords(indices,dims);
  newROIcoords = newROIcoords + repmat(minPos(1:3),[1,length(indices)]);
else
  newROIcoords = [];
end

return;

%%%%%%%%%%%%%%
% Debug/test %
%%%%%%%%%%%%%%

ROIcoords = [0; 0; 0];
ROIcoords = [1 2 3 4;
	         1 1 1 1;
	         1 1 1 1];
Xform = [1 0 0 0;
	     0 1 0 0;
	     0 0 1 0.5;
	     0 0 0 1];
inputVoxSize = [1,1,1];
outputVoxSize = [1,1,1/2];
xformROIcoords(ROIcoords,Xform,inputVoxSize,outputVoxSize)


function varargout = erfc(varargin)
%ERFC (overloaded)

switch class(varargin{1})

    case 'double'
        error('Overloaded SDPVAR/ERFC CALLED WITH DOUBLE. Report error')

    case 'sdpvar'
        varargout{1} = InstantiateElementWise(mfilename,varargin{:});

    case 'char'

        operator = struct('convexity','none','monotonicity','decreasing','definiteness','positive','model','callback');
        operator.bounds = @bounds;
        operator.range = [-1 1];
        operator.derivative =@(x)-exp(-x.^2)*2/sqrt(pi);
        varargout{1} = [];
        varargout{2} = operator;
        varargout{3} = varargin{3};

    otherwise
        error('SDPVAR/ERF called with CHAR argument?');
end

function [L,U] = bounds(xL,xU)
L = erfc(xL);
U = erfc(xU);


function pf = loadPoleFigure_dubna(fname,varargin)
% load dubna cnv file
%
% Syntax
%   pf = loadPoleFigure_dubna(fname)
%
% Input
%  fname - file name
%
% Options
%  STANDARD_GRID - use reguar 5 degree grid
%  DUBNA_GRID    - use original rotated Dubna grid
%
% Output
%  pf    - @PoleFigure
%
% See also
% loadPoleFigure dubna_demo ImportPoleFigureData

% ensure right extension
[pathstr, name, ext] = fileparts(fname); %#ok<ASGLU>
if ~any(strcmpi(ext,{'.cnv','.cns'}))
  interfaceError(fname);
end

% load data
try
  d = load(fname);
catch
  interfaceError(fname);
end

% ensure sufficients data
if ~isa(d,'double') || mod(numel(d),72)~=0 ||...
    numel(d)<3*72 || numel(d)>40*72
  interfaceError(fname);
end

d = reshape(d.',72,[]);
h = string2Miller(fname);

switch lower(ext)
  case '.cnv'
    r  = DubnaGrid(size(d,2));
  case '.cns'
    r = regularS2Grid('points',size(d),'maxtheta',5*degree*(size(d,2)-1),'antipodal');
end

pf = PoleFigure(h,r,d,varargin{:});


function [a,n,r,epe,eph,epw,usol_c,vsol_c] = mg_ns_q1cd(xy,ev,flowsol,domain,lncoarse,lnfine,outbnd)
%mg_ns_q1cd   convection-diffusion matrix generator for GMG (Navier-Stokes)
%   [a,n,r,epe,eph,epw,usol_c,vsol_c] = mg_ns_q1cd(xy,ev,flowsol,domain,lncoarse,lnfine,outbnd)
%   input
%           xy         vertex coordinate vector
%           ev         element mapping matrix
%           flowsol    current velocity field
%           domain     domain parameter, 1 for square, 3 for step
%           lncoarse   coarse grid index, log2(nc) for nc x nc square grid
%           lnfine     finest grid index, log2(nf) for nf x nf square grid
%           outbnd     location of outflow boundary
%   output
%           a          discrete diffusion operator
%           n          discrete convection operator
%           r          mass matrix
%           epe        viscosity normalised element peclet numbers
%           eph        flow specific element lengths
%           epw        centroid evaluated wind
%           usol_c     first convection coefficient on coarse grid nodes
%           vsol_c     second convection coefficient on coarse grid nodes
%
%   IFISS function: HCE; 18 March 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage

% Modification of DJS routine mg_q1cd to allow convection coefficients to
% be passed via flowsol.  Analogous to femq1_cd.

% Square domain
if domain==1,
% Size of fine grid, obtained from underlying flow solution flowsol
   nfine  = length(flowsol)/2;
   nf = sqrt(nfine);
   x=xy(:,1); y=xy(:,2);
   nvtx=length(x);   nel=length(ev(:,1));

% Get flow values at coarse grid points
   lshift = lnfine - lncoarse;
   usol=flowsol(1:nfine);
   usol_grid = reshape(usol,nf,nf);
   usol_grid_c = usol_grid(1:2^lshift:nf,1:2^lshift:nf);
   usol_c = reshape(usol_grid_c,nvtx,1);
   vsol=flowsol(nfine+1:2*nfine);
   vsol_grid = reshape(vsol,nf,nf);
   vsol_grid_c = vsol_grid(1:2^lshift:nf,1:2^lshift:nf);
   vsol_c = reshape(vsol_grid_c,nvtx,1);

% Step domain
elseif domain==3,
% Various counts of fine/coarse points in small part (1) and large part (2) of step
% Logic borrowed from AR routine step_mg_prolong
   nelf = 2^lnfine;
   nfine = (((outbnd+1)/2)*nelf+1)*(nelf+1) - (nelf/2)^2;
   nelc = 2^lncoarse;
   ncoarse = (((outbnd+1)/2)*nelc+1)*(nelc+1) - (nelc/2)^2;
   nfh1 = (nelf/2);     nfv1 = (nelf/2+1);  nf1 = nfh1*nfv1;
   nfh2 = (outbnd*nelf/2+1); nfv2 = nelf+1; nf2 = nfh2*nfv2;
   nch1 = (nelc/2);     ncv1 = (nelc/2+1);  nc1 = nch1*ncv1;
   nch2 = (outbnd*nelc/2+1); ncv2 = nelc+1; nc2 = nch2*ncv2;

% Get flow values at coarse grid points
   lshift = lnfine-lncoarse;
   usol=flowsol(1:nfine);
   usol1_grid = reshape(usol(1:nf1),nfh1,nfv1);
   usol1_grid_c = usol1_grid(1:2^lshift:nfh1,1:2^lshift:nfv1);
   usol1_c = reshape(usol1_grid_c,nc1,1);
   usol2_grid = reshape(usol(nf1+1:nfine),nfh2,nfv2);
   usol2_grid_c = usol2_grid(1:2^lshift:nfh2,1:2^lshift:nfv2);
   usol2_c = reshape(usol2_grid_c,nc2,1);
   usol_c = [usol1_c;usol2_c];
   vsol=flowsol(nfine+1:2*nfine);
   vsol1_grid = reshape(vsol(1:nf1),nfh1,nfv1);
   vsol1_grid_c = vsol1_grid(1:2^lshift:nfh1,1:2^lshift:nfv1);
   vsol1_c = reshape(vsol1_grid_c,nc1,1);
   vsol2_grid = reshape(vsol(nf1+1:nfine),nfh2,nfv2);
   vsol2_grid_c = vsol2_grid(1:2^lshift:nfh2,1:2^lshift:nfv2);
   vsol2_c = reshape(vsol2_grid_c,nc2,1);
   vsol_c = [vsol1_c;vsol2_c];
end

%
x=xy(:,1); y=xy(:,2);
nvtx=length(x);
nel=length(ev(:,1));
lx=max(x)-min(x); ly=max(y)-min(y);
hx=max(diff(x)); hy=max(diff(y));
%
% Initialise global matrices
a = sparse(nvtx,nvtx);
n = sparse(nvtx,nvtx);
r = sparse(nvtx,nvtx);
%
% Set up 2x2 Gauss points
gpt=1.0e0/sqrt(3.0e0);
s(1) = -gpt;  t(1) = -gpt;   wt(1) = 1;
s(2) =  gpt;  t(2) = -gpt;   wt(2) = 1;
s(3) =  gpt;  t(3) =  gpt;   wt(3) = 1;
s(4) = -gpt;  t(4) =  gpt;   wt(4) = 1;

%
% Inner loop over elements
for ivtx = 1:4
   xl_v(:,ivtx) = x(ev(:,ivtx));
   yl_v(:,ivtx) = y(ev(:,ivtx));
   xsl(:,ivtx) = usol_c(ev(:,ivtx));
   ysl(:,ivtx) = vsol_c(ev(:,ivtx));
end
ae = zeros(nel,4,4);
ne = zeros(nel,4,4);
re = zeros(nel,4,4);
% Loop over 2x2 Gauss points
for igpt = 1:4
   sigpt=s(igpt);
   tigpt=t(igpt);
   wght=wt(igpt);
%  Evaluate derivatives etc
   [jac,invjac,phi,dphidx,dphidy] = deriv(sigpt,tigpt,xl_v,yl_v);
   u_x = zeros(nel,1); u_y=zeros(nel,1);
   for k=1:4
      u_x(:) = u_x(:) + xsl(:,k) .* phi(:,k);
      u_y(:) = u_y(:) + ysl(:,k) .* phi(:,k);
   end
   for j = 1:4
      for i = 1:4
          ae(:,i,j) = ae(:,i,j)  + dphidx(:,i).*dphidx(:,j) .* invjac(:);
          ae(:,i,j) = ae(:,i,j)  + dphidy(:,i).*dphidy(:,j) .* invjac(:);
          re(:,i,j) = re(:,i,j)  + phi(:,i).*phi(:,j) .* jac(:);
%%%       ne(:,i,j) = ne(:,i,j) + flowx(:) .* phi(:,i) .* dphidx(:,j);
%%%       ne(:,i,j) = ne(:,i,j) + flowy(:) .* phi(:,i) .* dphidy(:,j);
          ne(:,i,j)  = ne(:,i,j)  + wght*u_x(:).*phi(:,i).*dphidx(:,j);
          ne(:,i,j)  = ne(:,i,j)  + wght*u_y(:).*phi(:,i).*dphidy(:,j);
       end
	end
% End of Gauss point loop
end
%
% Perform assembly of global matrix  and source vector
for krow=1:4
   nrow=ev(:,krow);
   for kcol=1:4
      ncol=ev(:,kcol);
      a = a + sparse(nrow,ncol,ae(:,krow,kcol),nvtx,nvtx);
      r = r + sparse(nrow,ncol,re(:,krow,kcol),nvtx,nvtx);
      n = n + sparse(nrow,ncol,ne(:,krow,kcol),nvtx,nvtx);
   end
end
%
% Computation of element Peclet number (at the centroid)
% Rectangle specific calculation here
hx=abs(xl_v(:,2)-xl_v(:,1)); hy=abs(yl_v(:,3)-yl_v(:,2));
[jac,invjac,phi,dphidx,dphidy] = deriv(0,0,xl_v,yl_v);
u_x = zeros(nel,1); u_y=zeros(nel,1);
for k=1:4
   u_x(:) = u_x(:) + xsl(:,k) .* phi(:,k);
   u_y(:) = u_y(:) + ysl(:,k) .* phi(:,k);
end
flowx = u_x(:);   flowy = u_y(:);
flow_l2 = sqrt(flowx(:) .* flowx(:) + flowy(:) .* flowy(:));
if all(flowx==0), flow_h=hy;
elseif all(flowy==0), flow_h=hx;
else
   angle = atan(abs(flowy./flowx));
   flow_h = min([hx./cos(angle),hy./sin(angle)],[],2);
end
eph = flow_h;
epe = flow_h.*flow_l2/2;
epw = flow_l2;


function rasterSegs = structFillin(rasterSegs1, scanNum, planC)
%"structFillin"
%   Take the rasterSegments from one structure and fill in all slices with
%   no defined structure.  Return the rasterSegments of the new filled in
%   structure.  Uses nearest neighbor interpolation to duplicate the
%   contours.
%
%   By JRA 1/7/05
%
%   rasterSegs1    : rasterSegments of a structure
%   planC          : CERR planC
%
%   rasterSegs     : rasterSegments of filled in structure
%
%Usage:
%   rasterSegs = structFillin(rasterSegs1, scanNum, planC)
%
% Copyright 2010, Joseph O. Deasy, on behalf of the CERR development team.
%
% This file is part of The Computational Environment for Radiotherapy Research (CERR).
%
% CERR development has been led by:  Aditya Apte, Divya Khullar, James Alaly, and Joseph O. Deasy.
%
% CERR has been financially supported by the US National Institutes of Health under multiple grants.
%
% CERR is distributed under the terms of the Lesser GNU Public License.
%
%     This version of CERR is free software: you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation, either version 3 of the License, or
%     (at your option) any later version.
%
% CERR is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
% without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
% See the GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with CERR.  If not, see <http://www.gnu.org/licenses/>.

indexS = planC{end};
rasterSegs = [];
[xV, yV, zV] = getScanXYZVals(planC{indexS.scan}(scanNum));

%sort input rasterSegments by CTSliceValue
rasterSegs1 = sortrows(rasterSegs1, 6);

%get list of CTSlices to iterate over.
slices1 = unique(rasterSegs1(:,6));
minSlice = min(slices1);
maxSlice = max(slices1);

%Iterate over all slices.  If no rasterSegs exist on a slice, find the
%nearest slice, take its rasterSegs, modify them to fit the new
%slice/zvalue, and add them to the rasterSegs list.
for sliceNum = minSlice:maxSlice
    rasterIndices = find(rasterSegs1(:,6) == sliceNum);
    if isempty(rasterIndices)
        sliceZValue = zV(sliceNum);
        nearestDefinedSliceNum = interp1(zV(slices1), slices1, sliceZValue, 'nearest');
        tmpIndV = find(rasterSegs1(:,6) == nearestDefinedSliceNum);
        tmpSegsV = rasterSegs1(tmpIndV,:);
        tmpSegsV(:,1) = zV(sliceNum);
        tmpSegsV(:,6) = sliceNum;
        rasterSegs = [rasterSegs;tmpSegsV];
    else
        rasterSegs = [rasterSegs;rasterSegs1(rasterIndices,:)];
    end
end


function ebsd = reduce(ebsd)
% reduce ebsd data by taking only second pixel
%
% Syntax
%   ebsd = reduce(ebsd)   % take every second pixel horiz. and vert.
%
% Input
%  ebsd - @EBSDhex
%
% Output
%  ebsd - @EBSDhex
%

s = size(ebsd);

rows = 1:2:s(1);
cols = 1:2:2*floor((s(2))/2);

[c,r] = meshgrid(cols,rows);

if ebsd.isRowAlignment
  c = c + iseven(round((r+1)/2));
else
  r = r + iseven(round((c+1)/2));
end

ind = r <= s(1) & c <= s(2) ;
ind = sub2ind(s,r(ind),c(ind));

ebsd.unitCell = 2*ebsd.unitCell;
ebsd.dHex = 2*ebsd.dHex;

ebsd = ebsd.subSet(ind);
ebsd = ebsd.gridify;

end


function tsat_values_test ( )

%*****************************************************************************80
%
%% TSAT_VALUES_TEST demonstrates the use of TSAT_VALUES.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 February 2009
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TSAT_VALUES_TEST:\n' );
  fprintf ( 1, '  TSAT_VALUES stores values of\n' );
  fprintf ( 1, '  the saturation temperature\n' );
  fprintf ( 1, '  as a function of pressure.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      P           Tsat(P)\n' );
  fprintf ( 1, '\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, p, tc ] = tsat_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fprintf ( 1, '  %12f  %24.16f\n', p, tc );

  end

  return
end


function update_plotTMPCPrediction(ax, handles, tPred, plot_tube, center_points, M_s, be_l, be_u, K_t, ...
    isConstrained, ub, lb, direction, isInput, TargetAvailable, target, LinearizationAvailable, lin)
% function plotStatePrediction
% Authors:      Martin Euler
%               Alexander Wischnewski
% Description:
%   helper function used to plot state predictions
% Inputs/parameters:
%   tPlot: vector containing the time window for plotting [tStart, tEnd]
%   tPred: vector with the time instants corresponding to the prediction
%   plot_tube: flag for tube plots
%   center_points: data for center points
%   M_s: tube shape matrices
%   be_l: lower bound of terminal set constraints
%   be_u: upper bound of terminal set constraints
%   K_t: tube controller
%   isConstrained: flag whether the signal is constrained or not
%   ub: upper limit for the signal
%   lb: lower limit for the signal
%   isInput: set to true in case this is an input (different calculation of tubes)
%   TargetAvailable: target trajectory available and should be plotted
%   target: actual target trajectory
%   LinearizationAvailable: linearization trajectory available and should be plotted
%   lin: actual linearization trajectory

% plot predicted signal
set(handles.pred, 'XData', tPred, 'YData', center_points);
% plot tube and terminal sets
if plot_tube
    % get tube bounds
    [y_low, y_up] = calcBoundsPlot(center_points, M_s, isInput, K_t, length(tPred), direction);
    % plot tube lower bounds
    set(handles.tube_ub, 'XData', tPred,'YData', y_low(1,:));
    % plot tube upper bounds
    set(handles.tube_lb, 'XData', tPred,'YData', y_up(1,:));
    % plot terminal set lower bounds and use a virtual time interval of one second (half a second
    % forth and back) to visualize it properly
    set(handles.terminal_lb, 'XData', [tPred(end)-0.5, tPred(end)+0.5],'YData',  be_l*ones(2, 1));
    % plot terminal set upper bounds
    set(handles.terminal_ub, 'XData', [tPred(end)-0.5, tPred(end)+0.5],'YData',  be_u*ones(2, 1));
end
if isConstrained
    set(handles.const_lb, 'XData', tPred, 'YData', ub);
    set(handles.const_ub, 'XData', tPred, 'YData', lb);
end
if TargetAvailable
    set(handles.target, 'XData', tPred, 'YData', target);
end
if LinearizationAvailable
    set(handles.lin, 'XData', tPred, 'YData', lin);
end

% adjust x limits appropriately such that it covers a certain range
xlim(ax, [tPred(1)-3, tPred(1)+5]);

end


function [x, f, eflag, output, lambda] = qps_glpk(H, c, A, l, u, xmin, xmax, x0, opt)
%QPS_GLPK  Linear Program Solver based on GLPK - GNU Linear Programming Kit.
%   [X, F, EXITFLAG, OUTPUT, LAMBDA] = ...
%       QPS_GLPK(H, C, A, L, U, XMIN, XMAX, X0, OPT)
%   [X, F, EXITFLAG, OUTPUT, LAMBDA] = QPS_GLPK(PROBLEM)
%   A wrapper function providing a standardized interface for using
%   GLPK to solve the following LP (linear programming) problem:
%
%       min C'*X
%        X
%
%   subject to
%
%       L <= A*X <= U       (linear constraints)
%       XMIN <= X <= XMAX   (variable bounds)
%
%   Inputs (all optional except H, C, A and L):
%       H : IGNORED dummy matrix of quadratic cost coefficients
%           for QP problems, which GLPK does not handle
%       C : vector of linear cost coefficients
%       A, L, U : define the optional linear constraints. Default
%           values for the elements of L and U are -Inf and Inf,
%           respectively.
%       XMIN, XMAX : optional lower and upper bounds on the
%           X variables, defaults are -Inf and Inf, respectively.
%       X0 : optional starting value of optimization vector X (NOT USED)
%       OPT : optional options structure with the following fields,
%           all of which are also optional (default values shown in
%           parentheses)
%           verbose (0) - controls level of progress output displayed
%               0 = no progress output
%               1 = some progress output
%               2 = verbose progress output
%           glpk_opt - options struct for GLPK, value in verbose
%                   overrides these options
%       PROBLEM : The inputs can alternatively be supplied in a single
%           PROBLEM struct with fields corresponding to the input arguments
%           described above: H, c, A, l, u, xmin, xmax, x0, opt
%
%   Outputs:
%       X : solution vector
%       F : final objective function value
%       EXITFLAG : exit flag, 1 - optimal, <= 0 - infeasible, unbounded or other
%       OUTPUT : output struct with the following fields:
%           errnum - GLPK errnum output arg
%           status - GKPK status output arg
%           runtime - solver run time in seconds
%       LAMBDA : struct containing the Langrange and Kuhn-Tucker
%           multipliers on the constraints, with fields:
%           mu_l - lower (left-hand) limit on linear constraints
%           mu_u - upper (right-hand) limit on linear constraints
%           lower - lower bound on optimization variables
%           upper - upper bound on optimization variables
%
%   Note the calling syntax is almost identical to that of GLPK. The main
%   difference is that the linear constraints are specified with A, L, U
%   instead of A, B, Aeq, Beq.
%
%   Calling syntax options:
%       [x, f, exitflag, output, lambda] = ...
%           qps_glpk([], c, A, l, u, xmin, xmax, x0, opt)
%
%       x = qps_glpk([], c, A, l, u)
%       x = qps_glpk([], c, A, l, u, xmin, xmax)
%       x = qps_glpk([], c, A, l, u, xmin, xmax, x0)
%       x = qps_glpk([], c, A, l, u, xmin, xmax, x0, opt)
%       x = qps_glpk(problem), where problem is a struct with fields:
%                       H, c, A, l, u, xmin, xmax, x0, opt
%                       all fields except 'c', 'A' and 'l' or 'u' are optional
%       x = qps_glpk(...)
%       [x, f] = qps_glpk(...)
%       [x, f, exitflag] = qps_glpk(...)
%       [x, f, exitflag, output] = qps_glpk(...)
%       [x, f, exitflag, output, lambda] = qps_glpk(...)
%
%   Example: (based on example from 'doc linprog')
%       c = [-5; -4; -6];
%       A = [ 1  -1   1;
%            -3  -2  -4;
%             3   2   0];
%       l = [-Inf; -42; -Inf];
%       u = [20; Inf; 30];
%       xmin = [0; 0; 0];
%       opt = struct('verbose', 2);
%       [x, f, s, out, lambda] = qps_glpk([], c, A, l, u, xmin, [], [], opt);
%
%   See also QPS_MASTER, GLPK.

%   MP-Opt-Model
%   Copyright (c) 2010-2020, Power Systems Engineering Research Center (PSERC)
%   by Ray Zimmerman, PSERC Cornell
%
%   This file is part of MP-Opt-Model.
%   Covered by the 3-clause BSD License (see LICENSE file for details).
%   See https://github.com/MATPOWER/mp-opt-model for more info.

%% check for Optimization Toolbox
% if ~have_feature('quadprog')
%     error('qps_glpk: requires the MEX interface to GLPK');
% end

%%----- input argument handling  -----
%% gather inputs
if nargin == 1 && isstruct(H)       %% problem struct
    p = H;
    if isfield(p, 'opt'),   opt = p.opt;    else,   opt = [];   end
    if isfield(p, 'x0'),    x0 = p.x0;      else,   x0 = [];    end
    if isfield(p, 'xmax'),  xmax = p.xmax;  else,   xmax = [];  end
    if isfield(p, 'xmin'),  xmin = p.xmin;  else,   xmin = [];  end
    if isfield(p, 'u'),     u = p.u;        else,   u = [];     end
    if isfield(p, 'l'),     l = p.l;        else,   l = [];     end
    if isfield(p, 'A'),     A = p.A;        else,   A = [];     end
    if isfield(p, 'c'),     c = p.c;        else,   c = [];     end
    if isfield(p, 'H'),     H = p.H;        else,   H = [];     end
else                                %% individual args
    if nargin < 9
        opt = [];
        if nargin < 8
            x0 = [];
            if nargin < 7
                xmax = [];
                if nargin < 6
                    xmin = [];
                end
            end
        end
    end
end

%% define nx, set default values for missing optional inputs
if isempty(H) || ~any(any(H))
    if isempty(A) && isempty(xmin) && isempty(xmax)
        error('qps_glpk: LP problem must include constraints or variable bounds');
    else
        if ~isempty(A)
            nx = size(A, 2);
        elseif ~isempty(xmin)
            nx = length(xmin);
        else    % if ~isempty(xmax)
            nx = length(xmax);
        end
    end
else
    error('qps_glpk: GLPK handles only LP problems, not QP problems');
    nx = size(H, 1);
end
if isempty(c)
    c = zeros(nx, 1);
end
if isempty(A) || (~isempty(A) && (isempty(l) || all(l == -Inf)) && ...
                                 (isempty(u) || all(u == Inf)))
    A = sparse(0,nx);           %% no limits => no linear constraints
end
nA = size(A, 1);                %% number of original linear constraints
if isempty(u)                   %% By default, linear inequalities are ...
    u = Inf(nA, 1);             %% ... unbounded above and ...
end
if isempty(l)
    l = -Inf(nA, 1);            %% ... unbounded below.
end
if isempty(xmin)                %% By default, optimization variables are ...
    xmin = -Inf(nx, 1);         %% ... unbounded below and ...
end
if isempty(xmax)
    xmax = Inf(nx, 1);          %% ... unbounded above.
end
if isempty(x0)
    x0 = zeros(nx, 1);
end

%% default options
if ~isempty(opt) && isfield(opt, 'verbose') && ~isempty(opt.verbose)
    verbose = opt.verbose;
else
    verbose = 0;
end

%% split up linear constraints
ieq = find( abs(u-l) <= eps );          %% equality
igt = find( u >=  1e10 & l > -1e10 );   %% greater than, unbounded above
ilt = find( l <= -1e10 & u <  1e10 );   %% less than, unbounded below
ibx = find( (abs(u-l) > eps) & (u < 1e10) & (l > -1e10) );
AA = [ A(ieq, :); A(ilt, :); -A(igt, :); A(ibx, :); -A(ibx, :) ];
bb = [ u(ieq);    u(ilt);    -l(igt);    u(ibx);    -l(ibx)];

%% grab some dimensions
nlt = length(ilt);      %% number of upper bounded linear inequalities
ngt = length(igt);      %% number of lower bounded linear inequalities
nbx = length(ibx);      %% number of doubly bounded linear inequalities
neq = length(ieq);      %% number of equalities
nie = nlt+ngt+2*nbx;    %% number of inequalities

ctype = [repmat('S', neq, 1); repmat('U', nlt+ngt+2*nbx, 1)];
vtype = repmat('C', nx, 1);

%% set options struct for GLPK
if ~isempty(opt) && isfield(opt, 'glpk_opt') && ~isempty(opt.glpk_opt)
    glpk_opt = glpk_options(opt.glpk_opt);
else
    glpk_opt = glpk_options;
end
glpk_opt.msglev = verbose;

%% call the solver
t0 = tic;
[x, f, errnum, extra] = ...
    glpk(c, AA, bb, xmin, xmax, ctype, vtype, 1, glpk_opt);
output.runtime = toc(t0);

%% set exit flag
if isfield(extra, 'status')             %% status found in extra.status
    output.errnum = errnum;
    output.status = extra.status;
    eflag = -errnum;
    if eflag == 0 && extra.status == 5
        eflag = 1;
    end
else                                    %% status found in errnum
    output.errnum = [];
    output.status = errnum;
    if have_feature('octave')
        if errnum == 180 || errnum == 151 || errnum == 171
            eflag = 1;
        else
            eflag = 0;
        end
    else
        if errnum == 5
            eflag = 1;
        else
            eflag = 0;
        end
    end
end

%% repackage lambdas
if isempty(extra) || ~isfield(extra, 'lambda') || isempty(extra.lambda)
    lambda = struct( ...
        'mu_l', zeros(nA, 1), ...
        'mu_u', zeros(nA, 1), ...
        'lower', zeros(nx, 1), ...
        'upper', zeros(nx, 1) ...
    );
else
    lam.eqlin = extra.lambda(1:neq);
    lam.ineqlin = extra.lambda(neq+(1:nie));
    lam.lower = extra.redcosts;
    lam.upper = -extra.redcosts;
    lam.lower(lam.lower < 0) = 0;
    lam.upper(lam.upper < 0) = 0;
    kl = find(lam.eqlin > 0);   %% lower bound binding
    ku = find(lam.eqlin < 0);   %% upper bound binding

    mu_l = zeros(nA, 1);
    mu_l(ieq(kl)) = lam.eqlin(kl);
    mu_l(igt) = -lam.ineqlin(nlt+(1:ngt));
    mu_l(ibx) = -lam.ineqlin(nlt+ngt+nbx+(1:nbx));

    mu_u = zeros(nA, 1);
    mu_u(ieq(ku)) = -lam.eqlin(ku);
    mu_u(ilt) = -lam.ineqlin(1:nlt);
    mu_u(ibx) = -lam.ineqlin(nlt+ngt+(1:nbx));

    lambda = struct( ...
        'mu_l', mu_l, ...
        'mu_u', mu_u, ...
        'lower', lam.lower(1:nx), ...
        'upper', lam.upper(1:nx) ...
    );
end


% compareBoundariesScript

savedir = '~/data/occlusion/labelme/figs/bnd_im';

folder = db(f).annotation.folder;
imfn = db(f).annotation.filename;
bn = strtok(imfn, '.');

im = imread(fullfile(imdir, folder, imfn));
figure(1), hold off, imagesc(im); axis image

load(fullfile(basedir, 'pb2', folder, [bn '_pb2_ucm']), 'ucm');
im = im2double(imresize(im, size(ucm), 'bilinear'));
imwrite(im, fullfile(savedir, imfn), 'Quality', 95);
grayim = rgb2gray(im2double(im));
h = 0*ones(size(grayim));
ucm = im2double(ucm);
pb = ucm / max(ucm(:));
tmpim = ordfilt2(max(pb, 0), 9, ones(3));
dispim = tmpim;%dispim = hsv2rgb(h, tmpim, 0.5*grayim+0.5*tmpim);
figure(2), hold off,  imagesc(dispim), axis image, colormap gray
imwrite(dispim, fullfile(savedir, [bn '_pb2_ucm.jpg']), 'Quality', 95);

load(fullfile(basedir, 'pb', folder, [bn '_pb']), 'pb');
grayim = rgb2gray(im2double(im));
h = 0*ones(size(grayim));
pb = pb / max(pb(:));
tmpim = ordfilt2(max(pb, 0), 9, ones(3));
dispim = tmpim;%dispim = hsv2rgb(h, tmpim, 0.5*grayim+0.5*tmpim);
figure(5), hold off,  imagesc(dispim), axis image
imwrite(dispim, fullfile(savedir, [bn '_pb.jpg']), 'Quality', 95);

load(fullfile(basedir, 'pb2', folder, [bn '_pb2']), 'pb');
grayim = rgb2gray(im2double(im));
h = 0*ones(size(grayim));
pb = pb / max(pb(:));
tmpim = ordfilt2(max(pb, 0), 9, ones(3));
dispim = tmpim;%dispim = hsv2rgb(h, tmpim, 0.5*grayim+0.5*tmpim);
figure(5), hold off,  imagesc(dispim), axis image
imwrite(dispim, fullfile(savedir, [bn '_pb2.jpg']), 'Quality', 95);

load(fullfile(occdir, folder, [bn '_occlusion']), 'bndinfo_all');
occ.po_all = getOcclusionMaps(bndinfo_all);
occim = mean(occ.po_all, 3);
%occim = im2double(imread(fullfile(basedir, 'results', folder, [strtok(imfn, '.') '_occ.jpg'])));
occim = occim/max(occim(:));
%h=0.65*ones(size(grayim));
tmpim = ordfilt2(occim, 9, ones(3));
dispim = tmpim;%dispim = hsv2rgb(h, tmpim, 0.5*grayim+0.5*tmpim);
figure(3), hold off, imagesc(dispim); axis image
imwrite(dispim, fullfile(savedir, [bn '_occave.jpg']), 'Quality', 95);

occim = occ.po_all(:, :, 1);
occim = occim/max(occim(:));
tmpim = ordfilt2(occim, 9, ones(3));
dispim = tmpim;%dispim = hsv2rgb(h, tmpim, 0.5*grayim+0.5*tmpim);
figure(6), hold off, imagesc(dispim); axis image
imwrite(dispim, fullfile(savedir, [bn '_occ1.jpg']), 'Quality', 95);

load(fullfile(basedir, 'labels2', folder, [bn '_labels']));
bmap = seg2bmap(lim, size(pb, 2), size(pb, 1));
bmap(:, [1:10 end-9:end]) = 0;
bmap([1:10 end-9:end], :) = 0;
tmpim = ordfilt2(double(bwmorph(bmap,'thin',inf)), 25, ones(5));
tmpim = cat(3, tmpim>0, zeros([size(tmpim) 2]));
dispim = im2double(im);
dispim(repmat(tmpim(:, :, 1)>0, [1 1 3])) = tmpim(repmat(tmpim(:, :, 1)>0, [1 1 3]));
%dispim = min(max(hsv2rgb(h, tmpim, 0.5*grayim+0.5*tmpim), 0), 1);
figure(4), hold off, imagesc(dispim); axis image,
imwrite(dispim, fullfile(savedir, [bn '_gt.jpg']), 'Quality', 95);

fid = fopen(fullfile(savedir, [bn '_ap.txt']), 'w');
fprintf(fid, 'index = %f   pb = %f   pb2+ucm = %f   occ1 = %f   occave = %f \n', sind(f), pr_pb(f).ap, pr_ucm(f).ap, pr_occ1(f).ap, pr_occave(f).ap);
fclose(fid);

disp([imfn ':  ' num2str([pr_pb2(f).ap pr_occave(f).ap])])

% grayim = imresize(rgb2gray(im2double(im)), size(pb), 'bilinear');
% resultim = repmat(grayim, [1 1 3]);
% tmpim = ordfilt2(max(pb,0), 9, ones(3));
% resultim((tmpim>0.05)) = tmpim(tmpim>0.05);
% tmpim = ordfilt2(occim, 9, ones(3));
% resultim(find(tmpim>0.05)+numel(tmpim)) = tmpim(tmpim>0.05);
% figure(4), hold off, imagesc(resultim), axis image


function [Cn, PNR] = correlation_pnr_parallel(obj, frame_range)
%% compute correlation image and pnr imaging in parallel
%% input:
%   frame_range: 1 X 2 vector indicating the starting and ending frames

%% Output:
%   Cn:     correlation image
%   PNR:    peak to noise ratio
%% Author: Pengcheng Zhou, Columbia University, 2017
%% email: zhoupc1988@gmail.com

%% process parameters

% map data
mat_data = obj.P.mat_data;

% dimension of data
dims = mat_data.dims;
d1 = dims(1);
d2 = dims(2);
T = dims(3);
obj.options.d1 = d1;
obj.options.d2 = d2;

% parameters for patching information
patch_pos = mat_data.patch_pos;
block_pos = mat_data.block_pos;

% number of patches
[nr_patch, nc_patch] = size(patch_pos);

% downsampling
ssub = obj.options.ssub;
tsub = obj.options.tsub;

% frames to be loaded for initialization
if ~exist('frame_range', 'var')
    frame_range = obj.frame_range;
end
if isempty(frame_range)
    frame_range = [1, T];
else
    frame_range(frame_range<1) = 1;
    frame_range(frame_range>T) = T;
end
T = diff(frame_range) + 1;
obj.frame_range = frame_range;

% parameters for detrending
if isfield(obj.options, 'nk') % number of knots for creating spline basis
    nk = obj.options.nk;
else
    nk = 1;
end
detrend_method = obj.options.detrend_method;

% parameter for avoiding using boundaries pixels as seed pixels
options = obj.options;

%% compute correlation image and pnr image in parallel
Cn = zeros(d1, d2);
PNR = zeros(d1, d2);

results = cell(nr_patch, nc_patch);

for m=1:(nr_patch*nr_patch)
    fprintf('|');
end
fprintf('\n');
parfor mpatch=1:(nr_patch*nc_patch)
    % get the indices corresponding to the selected patch
    tmp_patch = patch_pos{mpatch};
    tmp_block = block_pos{mpatch};

    % boundaries pixels to be avoided for detecting seed pixels
    tmp_options = options;

    % patch dimension
    tmp_d1 = diff(tmp_block(1:2))+1;
    tmp_d2 = diff(tmp_block(3:4))+1;
    tmp_options.gSig = tmp_options.gSig/ssub;
    tmp_options.gSiz = ceil(tmp_options.gSiz/ssub);

    % load the patch data
    Ypatch = get_patch_data(mat_data, tmp_patch, frame_range, true);
    if (ssub~=1)||(tsub~=1)
        Ypatch = dsData(Ypatch, tmp_options);
    end
    [tmp_options.d1, tmp_options.d2, T] = size(Ypatch);
    Ypatch = double(reshape(Ypatch, [], T));
    if nk>1
        Ypatch_dt = detrend_data(Ypatch, nk, detrend_method); % detrend data
        [tmp_Cn, tmp_PNR] = correlation_image_endoscope(Ypatch_dt, tmp_options);
    else
        [tmp_Cn, tmp_PNR] = correlation_image_endoscope(Ypatch, tmp_options);
    end
    if (ssub~=1)
        tmp_Cn = imresize(tmp_Cn, [tmp_d1, tmp_d2]);
        tmp_PNR = imresize(tmp_PNR, [tmp_d1, tmp_d2]);
    end
    % put everthing into one struct variable
    results{mpatch} = struct('Cn', tmp_Cn, 'PNR', tmp_PNR);
    fprintf('.');
end
fprintf('\n');

%% collect results
for mpatch=1:(nr_patch*nc_patch)
    % get the indices corresponding to the selected patch
    [mr, mc] = ind2sub([nr_patch, nc_patch], mpatch);
    tmp_patch = patch_pos{mr, mc};
    tmp_block = block_pos{mr, mc};
    r0 = tmp_block(1);
    r1 = tmp_block(2);
    c0 = tmp_block(3);
    c1 = tmp_block(4);
    ind_patch = true(r1-r0+1, c1-c0+1);
    ind_patch((tmp_patch(1):tmp_patch(2))-r0+1, (tmp_patch(3):tmp_patch(4))-c0+1) = false;

    % unpack results
    tmp_results = results{mpatch};
    %     eval(sprintf('tmp_results=results_patch_%d;', mpatch));
    tmp_Cn = tmp_results.Cn;
    tmp_PNR = tmp_results.PNR;

    Cn(r0:r1, c0:c1) = max(Cn(r0:r1, c0:c1), tmp_Cn.*(1-ind_patch));
    PNR(r0:r1, c0:c1) = max(PNR(r0:r1, c0:c1), tmp_PNR.*(1-ind_patch));
end


function failed_spm8

% WALLTIME 00:10:00
% MEM 3gb
% DEPENDENCY ft_volumenormalise ft_volumesegment ft_volumedownsample mni2tal tal2mni

mrifile = dccnpath('/home/common/matlab/fieldtrip/data/ftp/test/ctf/SubjectBraille.mri');
mri     = ft_read_mri(mrifile);

[ftver, ftpath] = ft_version;

% start with a clean path
rmpath(fullfile(ftpath, 'external', 'spm2'));
rmpath(fullfile(ftpath, 'external', 'spm8'));
rmpath(fullfile(ftpath, 'external', 'spm12'));

%% ft_volumenormalise

cfg            = [];
cfg.nonlinear  = 'no';

clear fun;
cfg.spmversion = 'spm2';
n2 = ft_volumenormalise(cfg, mri);
rmpath(spm('dir'));

clear fun;
cfg.spmversion = 'spm8';
n8 = ft_volumenormalise(cfg, mri);
rmpath(spm('dir'));

%% ft_volumesegment

clear fun;
cfg             = [];
cfg.spmversion  = 'spm2';
s2 = ft_volumesegment(cfg, mri);
rmpath(spm('dir'));

clear fun;
cfg.spmversion  = 'spm8';
s8 = ft_volumesegment(cfg, mri);
rmpath(spm('dir'));

%% ft_volumerealign

clear fun;
cfg               = [];
cfg.method        = 'spm';
cfg.spmversion    = 'spm2';
r2 = ft_volumerealign(cfg, mri, mri);
rmpath(spm('dir'));

clear fun;
cfg.spmversion    = 'spm8';
r8 = ft_volumerealign(cfg, mri, mri);
rmpath(spm('dir'));

%% ft_volumedownsample
tmp            = randn(256,256,256);
mri.pow        = tmp;
mri.pow(1)     = 0;

cfg            = [];
cfg.downsample = 2;

clear fun;
cfg.spmversion = 'spm2';
cfg.smooth     = 'no';
d2             = ft_volumedownsample(cfg, mri);
cfg.smooth     = 5;
d2s            = ft_volumedownsample(cfg, mri);
rmpath(spm('dir'));

clear fun;
cfg.spmversion = 'spm8';
cfg.smooth     = 'no';
d8             = ft_volumedownsample(cfg, mri);
cfg.smooth     = 5;
d8s            = ft_volumedownsample(cfg, mri);
rmpath(spm('dir'));

%% mni2tal and tal2mni

[ftver, ftpath] = ft_version;
cd(fullfile(ftpath, 'private'));

inpoints  = randn(100,3);
outpoints = mni2tal(inpoints);
rmpath(spm('dir'));

inpoint2s = tal2mni(outpoints);
rmpath(spm('dir'));

%% prepare_dipole_grid

%% prepare_mesh_segmentation


function [voxels,keep] = carve( voxels, camera )
%CARVE: remove voxels that are not in the silhouette
%
%   VOXELS = CARVE(VOXELS,CAMERA) carves away voxels that are not inside
%   the silhouette contained in CAMERA. The resulting voxel array is
%   returned.
%
%   [VOXELS,KEEP] = CARVE(VOXELS,CAMERA) also returns the indices of the
%   voxels that were retained.
%
%   Example:
%   >> camera = loadcameradata(1);
%   >> camera.Silhouette = getsilhouette( camera.Image );
%   >> voxels = carve( makevoxels(50), camera );
%   >> showscene( camera, voxels );
%
%   See also: LOADCAMERADATA
%             MAKEVOXELS
%             CARVEALL

%   Copyright 2005-2009 The MathWorks, Inc.
%   $Revision: 1.0 $    $Date: 2006/06/30 00:00:00 $

% Project into image
[x,y] = spacecarving.project( camera, voxels.XData, voxels.YData, voxels.ZData );

% Clear any that are out of the image
[h,w,d] = size(camera.Image); %#ok<NASGU>
keep = find( (x>=1) & (x<=w) & (y>=1) & (y<=h) );
x = x(keep);
y = y(keep);

% Now clear any that are not inside the silhouette
ind = sub2ind( [h,w], round(y), round(x) );
keep = keep(camera.Silhouette(ind) >= 1);

voxels.XData = voxels.XData(keep);
voxels.YData = voxels.YData(keep);
voxels.ZData = voxels.ZData(keep);
voxels.Value = voxels.Value(keep);


function [feat, unique_box_id] = spp_features_convX_to_poolX(spp_pooler, feat, boxes, random_scale, dedup)
% [feat, unique_box_id] = spp_features_convX_to_poolX(feat, boxes, random_scale, dedup)
%
%   boxes           in rows with (l, t, r, b)
%   random_scale    find best scale for each box or not
%   dedup           remove duplicate boxes on response map
%
% AUTORIGHTS
% ---------------------------------------------------------
% Copyright (c) 2014, Shaoqing Ren
%
% This file is part of the SPP code and is available
% under the terms of the Simplified BSD License provided in
% LICENSE. Please retain this notice and LICENSE if you use
% this file (or any portion of it) in your project.
% ---------------------------------------------------------

if nargin < 4
    random_scale = false;
end

if nargin < 5
    dedup = false;
end

assert(isfield(feat, 'rsp') && ~isempty(feat.rsp));

min_img_sz = min(feat.im_height, feat.im_width);

if isempty(boxes)
    feat = [];
    return;
end

feat.scale = feat.scale(:)';
if ~random_scale
    expected_scale = spp_pooler.expected_scale(min_img_sz, boxes, spp_pooler);

    [~, best_scale_ids] = min(abs(bsxfun(@minus, feat.scale, expected_scale(:))), [], 2);
else
    best_scale_ids = randi(length(feat.scale), size(boxes, 1), 1);
end

boxes_scales = feat.scale(best_scale_ids(:));
scaled_boxes = bsxfun(@times, (boxes - 1), (boxes_scales(:) - 1)) / (min_img_sz - 1) + 1;

if dedup
    rep_boxes = spp_pooler.response_boxes(scaled_boxes, spp_pooler);
    rsp_keys = [rep_boxes, best_scale_ids];
    [~, ia] = unique(rsp_keys, 'rows');
    unique_box_id = sort(ia);
else
    unique_box_id = 1:size(boxes, 2);
end

feat = spm_pool(feat.rsp, spp_pooler.spm_divs, scaled_boxes', best_scale_ids, ...
    spp_pooler.offset0, spp_pooler.offset, spp_pooler.step_standard);

end