%GRIDBDRE Generate grid edge boundary information.
%
%   [ BE, E, EV ] = GRIDBDRE( B, C ) Given arrays B and C with cell
%   connectivities and face boundary information generates an array
%   with edge boundary information.
%
%       Input       Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       b           (3/6,n_b)              Array with boundary information, the first row
%                                          indicates the cell number, followed by cell
%                                          face, boundary number, and optionally the
%                                          normal vectors for each boundary face
%       c           (n_pcell,n_cells)      Cell connectivity pointer array where the
%                                          entries in each column correspond to the
%                                          cell vertex numbers.
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       be          (5,n_b)                Array with boundary information, the first row
%                                          indicates the global cell number, followed by
%                                          local cell face, local edge, and global edge
%                                          and edge boundary numbers.
%       e           (n_e,n_c)              Array with global edge numbers for all cells.
%       ev          (n_e,2)                Array with indices to vertices for each edge.
%
%   See also GRIDBDR, GRIDEDGE

% Copyright 2013-2022 Precise Simulation, Ltd.


function H = PlotSwathe(bar,swathe,SwatheOpt)
% =======================================================================
% Plot a line with a shaded swathe (e.g for impulse responses, etc)
% =======================================================================
% H = PlotSwathe(bar,swathe,SwatheOpt)
% -----------------------------------------------------------------------
% INPUT
%   - bar: line to plot
%   - swathe: if a vector, draws symmetric swathe around bar. Otherwise
%       draws asymmetric swathe, with first columnn being the upper limit
%       and second column being the lower limit
% -----------------------------------------------------------------------
% OPTIONAL INPUT
%   - SwatheOpt.linecol: color of line, can be rgb number or string, e.g.
%       'blue'
%   - SwatheOpt.swathecol: color of swathe (can be rgb number or string,
%       e.g 'blue')
%   - SwatheOpt.transp: 1 for transparency (dflt=0)
%   - nticks: number ticks on the horizontal axis
%   - fo: first observation (convention: 1987.00 = 1987Q1)
%   - SwatheOpt.frequency: quarterly ('q') or annual ('y') [dflt='q']
%   - SwatheOpt.swatheonly: set to 1 to plot only the swathe (dflt=0)
% -----------------------------------------------------------------------
% OUPUT
%   - H.bar: handle for the median
%   - H.patch: handle for the patch
%   - H.swathe: H.swathe(1) handle for upp , H.swathe(2) handle for low
%   - H.ax: hande for the axis
%   - H.nobs: number of observations
% -----------------------------------------------------------------------
% EXAMPLE
% x = 3*(1:50);
% swathe = [1*(1:50);4*(1:50)];
% PlotSwathe(x,swathe)
% =======================================================================
% VAR Toolbox 3.0
% Ambrogio Cesa-Bianchi
% ambrogiocesabianchi@gmail.com
% March 2015. Updated November 2020
% -----------------------------------------------------------------------

if ~exist('SwatheOpt','var')
    SwatheOpt = PlotSwatheOption;
end

% Get the color of line
if isnumeric(SwatheOpt.linecol) % in case user provides already the rgb code
    linecol = SwatheOpt.linecol;
else % otherwise use rgb to get the code
    linecol = rgb(SwatheOpt.linecol);
end

% Get the color of swathe
if isnumeric(SwatheOpt.swathecol) % in case user provides already the rgb code
    swathecol = SwatheOpt.swathecol;
else % otherwise use rgb to get the code
    swathecol = rgb(SwatheOpt.swathecol);
end

% Number of observations
nobs = length(bar); H.nobs = nobs;

% Make sure data is row-vector
if size(bar,1)>1
    bar =bar';
end

% Check if swathe is vector and set error bands

if isvector(swathe)
    % Check if swathe has the right dimension [1 x T]
    if size(swathe,1)>1
        swathe = swathe';
        low = bar - swathe;
        upp = bar + swathe;
    else
        low = bar - swathe;
        upp = bar + swathe;
    end
else
    % Check if i has the right dimension [2 x T]
    if size(swathe,1)>2
        swathe = swathe';
        upp = swathe(1,:);
        low = swathe(2,:);
    else
        upp = swathe(1,:);
        low = swathe(2,:);
    end
end

% Create the x axis
xaxis = 1:nobs;

%% PLOT
% Initialize patch
edgeColor = swathecol+(1-swathecol)*0.55;
% Set saturation
swathealpha = SwatheOpt.alpha;
% SwatheOpt.transp to make the patch
if SwatheOpt.transp==1
    faceAlpha = swathealpha;
    patchColor = swathecol;
    set(gcf,'renderer','openGL')
else
    faceAlpha = 1;
    patchColor = swathecol+(1-swathecol)*(1-swathealpha);
    set(gcf,'renderer','painters')
end

% Plot the 'bar' line
if SwatheOpt.swatheonly==0
    H.bar = plot(xaxis,bar,'LineWidth',2,'Color',linecol,'Marker',SwatheOpt.marker);
end

% Add the error-bar plot elements
holdStatus = ishold;
if ~holdStatus, hold on,  end

% Make the coordinates for the patch
yP = [low,fliplr(upp)];
xP = [xaxis,fliplr(xaxis)];

% Remove any nans otherwise patch won't work
xP(isnan(yP))=[];
yP(isnan(yP))=[];
H.patch=patch(xP,yP,1,'facecolor',patchColor,'edgecolor','none','facealpha',faceAlpha);

% Make nice edges around the patch.
H.swathe(1)=plot(xaxis,low,'-','Color',edgeColor);
H.swathe(2)=plot(xaxis,upp,'-','Color',edgeColor);
if SwatheOpt.swatheonly==0
    delete(H.bar)
end
if SwatheOpt.swatheonly==0
    H.bar=plot(xaxis,bar,'LineWidth',2,'Color',linecol,'Marker',SwatheOpt.marker);
end
if ~holdStatus, hold off, end

% Make smart x labels
if SwatheOpt.do_dates==1
    DatesPlot(fo,nobs,nticks,SwatheOpt.frequency)
end
H.ax = gca;


function hewma2_plot(Zpop,mu,cp,cp_ind,ooc_indices,maxtime,tthresh,tt,tm,q,Wm,Zm,sterr,varargin)
% :Usage:
% ::
%
%     hewma2_plot(Zpop,mu,cp,cp_ind,ooc_indices,maxtime,tthresh,tt,tm,q,Wm,Zm,sterr,varargin)
%
% see hewma2.m

cp = round(cp);

dogroupplot = 0;
tagname  = 'hewma2display';

if length(varargin) > 0
    dogroupplot = 1;
    tagname  = 'hewma2diffdisplay';
end

f1 = findobj('Tag', tagname);
if isempty(f1)
    f1 = tor_fig(2,2);
    set(f1, 'Tag', tagname);
else
    figure(f1)
    for i = 1:4, subplot(2, 2, i); cla; end
end

ZZ = Zpop-mu;

% ----------------------------------------------------------
% *
% * PLOT 1: GROUP AVERAGE (OR DIFFERENCE)
% *
% ----------------------------------------------------------
subplot(2, 2, 1);

hh(1) = plot(ZZ,'k','LineWidth',2);

% baseline box
% ------------------------------
    yl = get(gca,'YLim');
    cl = tthresh .* mean(sterr);
    yl(1) = min([yl(1)-1 -cl-1]); yl(2) = max([yl(2)+1 cl+1]);

    fill([1 tt tt 1],[yl(1) yl(1) yl(2) yl(2)],[.85 .85 .85]);
    text(5,min(ZZ),'Baseline','FontSize',16);

% group Z stat
% ------------------------------
hh(1) = plot(ZZ,'k','LineWidth',2);
hold on

 % change points
% ------------------------------
    if ~isnan(cp)
        plot(cp,ZZ(cp),'bo','MarkerSize',8,'MarkerFaceColor','g');
        plot([cp cp],get(gca,'YLim'),'g','LineWidth',2);
    end

    % individual change points
    if ~isempty(cp_ind)
        if ~any(isnan(cp_ind))
            plot(cp_ind,ZZ(cp_ind),'go','MarkerSize',6);
        end
    end

% OOC points in red
% ------------------------------
    % all ooc points
    ZZtmp = NaN * zeros(size(ZZ)); ZZtmp(ooc_indices) = ZZ(ooc_indices);
    plot(ZZtmp,'r','LineWidth',2);

% max t value in blue
% ------------------------------

    if ~isnan(maxtime)
        plot(maxtime,ZZ(maxtime),'go','MarkerSize',8,'MarkerFaceColor','b');
    end

 % standard error
% ------------------------------
    fill_around_line(ZZ,sterr,'k');

    title('Group activation');
    if length(varargin) > 0, title('Difference between groups'); end

 % control limits
% ------------------------------
    cl = tthresh .* mean(sterr);
    hh(2) = plot(get(gca,'XLim'),[cl cl],'k--');
    plot(get(gca,'XLim'),[-cl -cl],'k--');
    legend(hh,'Group mean','Control limit (approximate)');

% ----------------------------------------------------------
% *
% * PLOT 2: NULL HYPOTHESIS DISTRIBUTION
% *
% ----------------------------------------------------------

    subplot(2,2,2);
    hist(q,50); hold on; plot([abs(tm) abs(tm)],get(gca,'YLim'),'k-','LineWidth',2);
    title('Null-hypothesis max t-value and observed max t-value');

% ----------------------------------------------------------
% *
% * PLOT 3: SUBJECT WEIGHTS
% *
% ----------------------------------------------------------

    subplot(2,2,3);
    n = 1:length(Wm);
    plot([n;n],[Wm';Wm'],'o','LineWidth',2);
    title('Case weights');

% ----------------------------------------------------------
% *
% * PLOT 4: INDIVIDUAL SUBJECTS (OR GROUPS)
% *
% ----------------------------------------------------------

    subplot(2,2,4);

    % group contrast, or individuals
    if dogroupplot
        gfits = varargin{1};
        plot(gfits');
        title('Groups');
        legend({'Low' 'High'});
        yval = max(gfits(:)) + .01*max(gfits(:));
        %yval = repmat(yval,1,length(ZZtmp));
        ZZtmp(~isnan(ZZtmp)) = yval;
        hold on;

        plot(ZZtmp,'r','LineWidth',3);

        if ~isnan(cp)
            plot([cp cp],get(gca,'YLim'),'g','LineWidth',2);
        end

        % baseline box
        yl = get(gca,'YLim');
        fill([1 tt tt 1],[yl(1) yl(1) yl(2) yl(2)],[.85 .85 .85]);
        text(5,min(ZZ),'Baseline','FontSize',16);

    else
        plot(Zm');
        title('Individual cases');
        drawnow
    end

    return


function [ y2, m2, d2, f2 ] = ymdf_inc_republican ( y1, m1, d1, f1, days )

%*****************************************************************************80
%
%% YMDF_INC_REPUBLICAN increments a Republican YMDF date by DAYS days.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 April 2103
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y1, M1,  D1, real F1,
%    the YMDF date.
%
%    Input, real DAYS, the number of days to advance the date.
%
%    Output, integer Y2, M2, D2, real F2,
%    the incremented YMDF date.
%

%
%  Copy the parameters.
%
  y2 = y1;
  m2 = m1;
  d2 = d1;
  f2 = f1 + days;
%
%  Check the parameters.
%
  [ y2, m2, d2, f2, ierror ] = ymdf_check_republican ( y2, m2, d2, f2 );

  return
end


%%*******************************************************************
%% HSDHKMrhsfun: compute the right-hand side vector of the
%%               Schur complement equation for the  HKM direction.
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%*******************************************************************

function [rhs,EinvRc,hRd] = HSDHKMrhsfun(blk,At,par,X,Z,rp,Rd,sigmu,hRd,dX,dZ)

m = par.m;
if (nargin > 8)
    corrector = 1;
else
    corrector = 0;
    hRd = zeros(m+2,1);
end
hEinvRc = zeros(m+2,1);
EinvRc = cell(size(blk,1),1);
if length(sigmu)==1; sigmu = sigmu*ones(1,size(blk,1)); end
%%
for p = 1:size(blk,1)
    pblk = blk(p,:);
    n = sum(pblk{2});
    if strcmp(pblk{1},'l')
        if (corrector)
            Rq = dX{p}.*dZ{p};
        else
            Rq = sparse(n,1);
            tmp  = par.dd{p}.*Rd{p};
            tmp2 = mexMatvec(At{p},tmp,1);
            hRd = hRd + tmp2;
        end
        EinvRc{p} = sigmu(p)./Z{p}-X{p} -Rq./Z{p};
        tmp2 = mexMatvec(At{p,1},EinvRc{p},1);
        hEinvRc = hEinvRc + tmp2;
    elseif strcmp(pblk{1},'q')
        if (corrector)
            ff{p} = qops(pblk,1./par.gamz{p},Z{p},3); %#ok
            hdx = qops(pblk,par.gamz{p},ff{p},5,dX{p});
            hdz = qops(pblk,par.gamz{p},ff{p},6,dZ{p});
            hdxdz = Arrow(pblk,hdx,hdz);
            Rq = qops(pblk,par.gamz{p},ff{p},6,hdxdz);
        else
            Rq  = sparse(n,1);
            tmp = par.dd{p}.*Rd{p} ...
                + qops(pblk,qops(pblk,Rd{p},par.Zinv{p},1),X{p},3) ...
                + qops(pblk,qops(pblk,Rd{p},X{p},1),par.Zinv{p},3);
            tmp2 = mexMatvec(At{p,1},tmp,1);
            hRd = hRd + tmp2;
        end
        EinvRc{p} = sigmu(p)*par.Zinv{p}-X{p} -Rq;
        tmp2 = mexMatvec(At{p,1},EinvRc{p},1);
        hEinvRc = hEinvRc + tmp2;
    elseif strcmp(pblk{1},'s')
        if (corrector)
            Rq = Prod3(pblk,dX{p},dZ{p},par.Zinv{p},0);
            Rq = 0.5*(Rq+Rq');
        else
            Rq = sparse(n,n);
            tmp = Prod3(pblk,X{p},Rd{p},par.Zinv{p},0,par.nzlistAy{p});
            EinvRc{p} = 0.5*(tmp+tmp');
            tmp2 = AXfun(pblk,At(p,:),par.permA(p,:),EinvRc(p));
            hRd = hRd + tmp2;
        end
        EinvRc{p} = sigmu(p)*par.Zinv{p}-X{p}- Rq;
        tmp2 = AXfun(pblk,At(p,:),par.permA(p,:),EinvRc(p));
        hEinvRc = hEinvRc + tmp2;
    end
end
%%
rhs = rp + hRd - hEinvRc;
rhs(m+1) = rhs(m+1) + (par.mu/par.tau - par.kap);
if (corrector)
    rhs(m+1) = rhs(m+1) - par.dtau*par.dkap/par.tau;
end
%%*******************************************************************


% Copyright (C) 2017-2018 Titus Cieslewski, RPG, University of Zurich,
%   Switzerland
%   You can contact the author at <titus at ifi dot uzh dot ch>
% Copyright (C) 2017-2018 Siddharth Choudhary, College of Computing,
%   Georgia Institute of Technology, Atlanta, GA, USA
% Copyright (C) 2017-2018 Davide Scaramuzza, RPG, University of Zurich,
%   Switzerland
%
% This file is part of dslam_open.
%
% dslam_open 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.
%
% dslam_open 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 dslam_open. If not, see <http://www.gnu.org/licenses/>.

function [num_frames, connected_ate, decentr_state] = getConnectedAte(...
    decentr_state, group)
%% Poses to positions ground truth.
p_gt_C = [];
for i = group
    p_gt_C = [p_gt_C; cell2mat(cellfun(@(x) x(1:3, 4)', ...
        decentr_state{i}.gt_T_W_C, 'UniformOutput', false))];
end
%% Poses to positions group estimate.
p_gW_C = [];
for i = group
    T_gW_C = cellfun(@(x) decentr_state{i}.Sim_W_O * x, ...
        decentr_state{i}.Sim_O_C, 'UniformOutput', false);
    p_gW_C = [p_gW_C; cell2mat(cellfun(@(x) x(1:3, 4)', ...
        T_gW_C, 'UniformOutput', false))];
end

%% Optimize: min error between positions, func of T_gt_O_ate
[decentr_state{min(group)}.T_gt_O_ate, connected_ate] = ...
    alignTrajs(p_gt_C', p_gW_C', decentr_state{min(group)}.T_gt_O_ate);
num_frames = size(p_gW_C, 1);

end


function obj = t1_pnp_func_new(in1,in2,in3)
coefs_tq1_1 = in2(1);
coefs_tq1_2 = in2(4);
coefs_tq2_1 = in2(2);
coefs_tq1_3 = in2(7);
coefs_tq2_2 = in2(5);
coefs_tq3_1 = in2(3);
coefs_tq1_4 = in2(10);
coefs_tq2_3 = in2(8);
coefs_tq3_2 = in2(6);
coefs_tq1_5 = in2(13);
coefs_tq2_4 = in2(11);
coefs_tq3_3 = in2(9);
coefs_tq1_6 = in2(16);
coefs_tq2_5 = in2(14);
coefs_tq3_4 = in2(12);
coefs_tq1_7 = in2(19);
coefs_tq2_6 = in2(17);
coefs_tq3_5 = in2(15);
coefs_tq1_8 = in2(22);
coefs_tq2_7 = in2(20);
coefs_tq3_6 = in2(18);
coefs_tq1_9 = in2(25);
coefs_tq2_8 = in2(23);
coefs_tq3_7 = in2(21);
coefs_tq2_9 = in2(26);
coefs_tq3_8 = in2(24);
coefs_tq3_9 = in2(27);
coefs_tq1_10 = in2(28);
coefs_tq2_10 = in2(29);
coefs_tq3_10 = in2(30);
pinvG1_1 = in1(1);
pinvG1_2 = in1(4);
pinvG1_3 = in1(7);
q0 = in3(1,:);
q1 = in3(2,:);
q2 = in3(3,:);
q3 = in3(4,:);
obj = q0.^2.*(coefs_tq1_1.*pinvG1_1+coefs_tq2_1.*pinvG1_2+coefs_tq3_1.*pinvG1_3)+q1.^2.*(coefs_tq1_5.*pinvG1_1+coefs_tq2_5.*pinvG1_2+coefs_tq3_5.*pinvG1_3)+q2.^2.*(coefs_tq1_8.*pinvG1_1+coefs_tq2_8.*pinvG1_2+coefs_tq3_8.*pinvG1_3)+q3.^2.*(coefs_tq1_10.*pinvG1_1+coefs_tq2_10.*pinvG1_2+coefs_tq3_10.*pinvG1_3)+q0.*q1.*(coefs_tq1_2.*pinvG1_1+coefs_tq2_2.*pinvG1_2+coefs_tq3_2.*pinvG1_3)+q0.*q2.*(coefs_tq1_3.*pinvG1_1+coefs_tq2_3.*pinvG1_2+coefs_tq3_3.*pinvG1_3)+q0.*q3.*(coefs_tq1_4.*pinvG1_1+coefs_tq2_4.*pinvG1_2+coefs_tq3_4.*pinvG1_3)+q1.*q2.*(coefs_tq1_6.*pinvG1_1+coefs_tq2_6.*pinvG1_2+coefs_tq3_6.*pinvG1_3)+q1.*q3.*(coefs_tq1_7.*pinvG1_1+coefs_tq2_7.*pinvG1_2+coefs_tq3_7.*pinvG1_3)+q2.*q3.*(coefs_tq1_9.*pinvG1_1+coefs_tq2_9.*pinvG1_2+coefs_tq3_9.*pinvG1_3);


function cout = surrogate3(cin)

%tstoolbox/@core/surrogate3
%   Syntax:
%     * cout = surrogate3(cin)
%
%   Input Arguments:
%     * cin - core object
%
%   create surrogate data for a scalar time series by permuting samples
%   randomly
%
% Copyright 1997-2001 DPI Goettingen, License http://www.physik3.gwdg.de/tstool/gpl.txt

if ndim(cin) > 1
	error('not a scalar time series')
end

y = data(cin);

len = dlens(cin,1);

cout = core(y(randperm(len)));


%% pro_Create: Create a new project structure
%
% Usage:
%   pro = pro_Create()
%
% Inputs:
%
% Output:
%     pro                project structure
%                          .Inputs.pdfs: cell-array of the model inputs with the pdf handles
%                          .Inputs.Names: cell-array with the input names
%                          .N: scalar, number of samples of crude Monte Carlo
%                          .Model.handle: handle to the model function
%                          .Model.Name: string name of the model
%
% ------------------------------------------------------------------------
% See also
%
% Author : Flavio Cannavo'
% e-mail: flavio(dot)cannavo(at)gmail(dot)com
% Release: 1.0
% Date   : 28-01-2011
%
% History:
% 1.0  28-01-2011  First release.
%%

function pro = pro_Create()

pro.Inputs.pdfs = {};
pro.Inputs.Names = {};
pro.N = 10000;
pro.Model.handle = [];
pro.Model.Name = [];


function [divTr, divTe] = sample_leaveOneBlockOut(label, block_idx, strat)
%SAMPLE_LEAVEONEBLOCKOUT - Sampling function: block divisions
%
%Synopsis:
%  [DIVTR, DIVTE] = sample_leaveOneBlockOut(LABEL, BLOCK_IDX, <STRAT>)
%
%Arguments:
% LABEL     - Despensable input argument, required for compability reasons
% BLOCK_IDX - [1 x nSamples] array of indices which defines to which block
%             each sample belongs. This can be e.g. mrk.blkno, when mrk is
%             obtained from mrk_evenlyInBlocks
% STRAT     - [1 x 2] bool indicating to stratify the samples by
%             performing a random sampling on the training and/or test
%             sets, default [0 0]
%
%Returns:
% DIVTR     - Partitions of the training set
% DIVTE     - Partitions of the test set

% 2015-11 Matthias Schultze-Kraft

if nargin<3
    strat = [0 0];
end

divTr = cell(1, 1);
divTe = cell(1, 1);
idx_list = unique(block_idx);
for nn = 1:length(idx_list)
    % test set indices
    idx = find(block_idx==idx_list(nn));
    if strat(2)
        idx = stratify(idx,label);
    end
    divTe{1}(nn) = {idx};
    % training set indices
    idx = find(block_idx~=idx_list(nn));
    if strat(1)
        idx = stratify(idx,label);
    end
    divTr{1}(nn) = {idx};
end

function idx = stratify(idx,label)
idx = {idx(logical(label(1,idx))) idx(logical(label(2,idx)))};
Nmin = min([length(idx{1}) length(idx{2})]);
if Nmin==0
    warning('No stratification performed because the number of samples of one class is zero.')
else
    [Nmax,ci] = max([length(idx{1}) length(idx{2})]);
    ri = randperm(Nmax,Nmin);
    idx{ci} = idx{ci}(ri);
end
idx = [idx{:}];


% [BOUNDX,BOUNDY] = LABEL_OPEN_EDGES(RGBI)
%
% Input: RGBI is a RGB image we want to label edges in.
% This program allows the user to input any number of open
% edges. Here is the interactive usage:
% label_open_edges commands:
% LEFT CLICK with the mouse to add a new point.
% RIGHT CLICK with the mouse to add a new point on an image edge.
% MIDDLE CLICK with the mouse to finish the curve.
% Press ENTER to finish the curve.
% Press <CONTROL> z to undo the last curve piece added.
% Press H for a list of commands.
% Press z to zoom out.
% Press Z to put in zoomin mode, then draw a rectangle to zoomin on.
% Press - to decrease the first edge threshold.
% Press = to increase the first edge threshold.
% Press _ to decrease the second edge threshold.
% Press + to increase the second edge threshold.
% Press t to see the image with/without the edges.
% Press <CONTROL> q to quit.

function varargout = label_open_edges(arg1)

if nargin ~= 1,
  error('LABEL_OPEN_EDGES: Usage: label_open_edges(rgbI)');
end;

if strcmp(arg1,'Move'),
  move;
  return;
elseif strcmp(arg1,'ButtonDown'),
  buttondown;
  return;
elseif strcmp(arg1,'Drag'),
  drag;
  return;
elseif strcmp(arg1,'ButtonUp'),
  buttonup;
  return;
elseif strcmp(arg1,'KeyPress'),
  keypress;
  return;
end;

fprintf('Label edges: Type ''H'' for a list of commands.\n');

global FIG_H AXIS_H IMAGE_H NROWS NCOLS;
global BOUNDX BOUNDY CURVEN LASTLINE_H CURVEPIECESTART;
global EDGEI RGBI RGBI0 DISTEDGEI NNEDGEI THRESH ISEDGESHOWN;

% rgb image
RGBI0 = im2double(arg1);
RGBI = RGBI0;
[NROWS,NCOLS,ncolors] = size(RGBI0);

[EDGEI,THRESH] = edge(rgb2gray(RGBI0),'canny');

% draw the edges on the image
RGBI = reshape(RGBI,[NROWS*NCOLS],ncolors);
RGBI(EDGEI,1) = 1; RGBI(EDGEI,2) = .75; RGBI(EDGEI,3) = 1;
RGBI = reshape(RGBI,[NROWS,NCOLS,ncolors]);

% draw the image
clf;
image(RGBI); axis('image'); hold on;

% find the closest edge to each pixel
[DISTEDGE,NNEDGEI] = bwdist(EDGEI);

% save which image we are looking at
FIG_H = gcf;
AXIS_H = gca;
IMAGE_H = findobj(AXIS_H,'type','image');

% we will use IMAGE_H's UserData as a flag to see when we are done
set(IMAGE_H,'UserData',1);

% Initialize the lines drawn
BOUNDX{1} = []; BOUNDY{1} = [];
CURVEN = 1;
LASTLINE_H = [];
CURVEPIECESTART = [];
ISEDGESHOWN = 1;

% wait for the mouse to be pushed
set(IMAGE_H,'ButtonDownFcn','label_open_edges(''ButtonDown'')');
set(FIG_H,'WindowButtonMotionFcn','label_open_edges(''Move'')');

% or for a key to be pressed
set(FIG_H,'KeyPressFcn','label_open_edges(''KeyPress'')');

waitfor(IMAGE_H,'UserData','Done');

varargout = {BOUNDX , BOUNDY};

clear global BOUNDX BOUNDY;

% mouse moved
function move

global FIG_H IMAGE_H AXIS_H;
global BOUNDX BOUNDY CURVEN LASTLINE_H;
global EDGEI RGBI DISTEDGEI NNEDGEI;
global EDGEPOINTER_H;

% find the mouse location
[imageHandle,x,y] = over_image(AXIS_H);
if imageHandle ~= IMAGE_H | imageHandle == 0, return; end;

% find the closest edge point
[yedge,xedge] = ind2sub(size(EDGEI),NNEDGEI(round(y),round(x)));
if isempty(EDGEPOINTER_H) | ~ishandle(EDGEPOINTER_H),
  EDGEPOINTER_H = plot(xedge,yedge,'b.','markersize',12,'hittest','off');
else,
  set(EDGEPOINTER_H,'XData',xedge,'YData',yedge);
end;

% mouse button pressed down
function buttondown

global FIG_H IMAGE_H AXIS_H;
global BOUNDX BOUNDY CURVEN LASTLINE_H CURVEPIECESTART;
global EDGEI RGBI DISTEDGEI NNEDGEI;

% which mouse button was pushed?
selection_type = get(FIG_H,'SelectionType');

if strcmp(selection_type,'normal'),
  b = 1;
elseif strcmp(selection_type,'extend'),
  b = 2;
elseif strcmp(selection_type,'alt'),
  b = 3;
end;

% left mouse button: input a new point
if b == 1,
  % find the point clicked
  [imageHandle,x,y] = over_image(AXIS_H);
  if imageHandle ~= IMAGE_H | imageHandle == 0, return; end;

  i = length(BOUNDX{CURVEN});
  % add the point to the current curve
  BOUNDX{CURVEN} = [BOUNDX{CURVEN} ; x];
  BOUNDY{CURVEN} = [BOUNDY{CURVEN} ; y];

  % plot the new point
  if isempty(LASTLINE_H),
    LASTLINE_H = plot(BOUNDX{CURVEN},BOUNDY{CURVEN},...
		      'r','erasemode','none','hittest','off');
    CURVEPIECESTART = 1;
  else,
    set(LASTLINE_H,'XData',BOUNDX{CURVEN},'YData',BOUNDY{CURVEN});
    CURVEPIECESTART = [CURVEPIECESTART;i+1];
  end;

  set(FIG_H,'WindowButtonMotionFcn','label_open_edges(''Drag'')',...
	    'WindowButtonUpFcn','label_open_edges(''ButtonUp'')');
  set(IMAGE_H,'ButtonDownFcn','');

% right mouse button: input a new point on an edge
elseif b == 3,

  % input a point
  [imageHandle,x,y] = over_image(AXIS_H);
  if imageHandle ~= IMAGE_H | imageHandle == 0, return; end;

  i = length(BOUNDX{CURVEN});

  % find the closest edge point to the boundary point
  [yedge,xedge] = ind2sub(size(EDGEI),NNEDGEI(round(y),round(x)));

  % add the point to the current curve
  BOUNDX{CURVEN} = [BOUNDX{CURVEN} ; xedge];
  BOUNDY{CURVEN} = [BOUNDY{CURVEN} ; yedge];

  % plot the new point
  if isempty(LASTLINE_H),
    LASTLINE_H = plot(BOUNDX{CURVEN},BOUNDY{CURVEN},...
		      'r','erasemode','none','hittest','off');
    CURVEPIECESTART = 1;
  else,
    set(LASTLINE_H,'XData',BOUNDX{CURVEN},'YData',BOUNDY{CURVEN});
    CURVEPIECESTART = [CURVEPIECESTART;i+1];
  end;

  set(FIG_H,'WindowButtonMotionFcn','label_open_edges(''Drag'')',...
	    'WindowButtonUpFcn','label_open_edges(''ButtonUp'')');
  set(IMAGE_H,'ButtonDownFcn','');

% Done entering the current curve
elseif b == 2,
  set(LASTLINE_H,'color',[.75,.75,1],'linewidth',4);
  CURVEN = CURVEN + 1;
  LASTLINE_H = [];
  BOUNDX{CURVEN} = [];
  BOUNDY{CURVEN} = [];
end;

% drag the mouse
function drag

global EDGEI RGBI DISTEDGEI NNEDGEI;
global FIG_H AXIS_H IMAGE_H BOUNDX BOUNDY CURVEN LASTLINE_H;

% which mouse button was pushed?
selection_type = get(FIG_H,'SelectionType');
if strcmp(selection_type,'normal'),
  b = 1;
elseif strcmp(selection_type,'extend'),
  b = 2;
elseif strcmp(selection_type,'alt'),
  b = 3;
end;

[imageHandle,x,y] = over_image(AXIS_H);

if imageHandle ~= 0 & imageHandle == IMAGE_H & (b == 1 | b == 3),
  % left mouse button: input a new point
  if b == 1,
    % set control point to the current mouse location
    BOUNDX{CURVEN} = [BOUNDX{CURVEN} ; x];
    BOUNDY{CURVEN} = [BOUNDY{CURVEN} ; y];
  elseif b == 3,
    % right mouse button: input a new point on an edge
    [yedge,xedge] = ind2sub(size(EDGEI),NNEDGEI(round(y),round(x)));
    BOUNDX{CURVEN} = [BOUNDX{CURVEN} ; xedge];
    BOUNDY{CURVEN} = [BOUNDY{CURVEN} ; yedge];
  end;
  set(LASTLINE_H,'XData',BOUNDX{CURVEN},'YData',BOUNDY{CURVEN});
end;

% mouse button up
function buttonup

global EDGEI RGBI DISTEDGEI NNEDGEI;
global IMAGE_H AXIS_H FIG_H BOUNDX BOUNDY CURVEN LASTLINE_H;
selection_type = get(FIG_H,'SelectionType');
if strcmp(selection_type,'normal'),
  b = 1;
elseif strcmp(selection_type,'extend'),
  b = 2;
elseif strcmp(selection_type,'alt'),
  b = 3;
end;

[imageHandle,x,y] = over_image(AXIS_H);
if imageHandle ~= 0,
  if imageHandle == IMAGE_H,
    if b == 2, return;
    elseif b == 1,
      % set control point to the current mouse location
      BOUNDX{CURVEN} = [BOUNDX{CURVEN} ; x];
      BOUNDY{CURVEN} = [BOUNDY{CURVEN} ; y];
    elseif b == 3,
      % right mouse button: input a new point on an edge
      [yedge,xedge] = ind2sub(size(EDGEI),NNEDGEI(round(y),round(x)));
      BOUNDX{CURVEN} = [BOUNDX{CURVEN} ; xedge];
      BOUNDY{CURVEN} = [BOUNDY{CURVEN} ; yedge];
    end;
    set(LASTLINE_H,'XData',BOUNDX{CURVEN},'YData',BOUNDY{CURVEN});
  end;
end;

set(FIG_H,'WindowButtonUpFcn','',...
	  'WindowButtonMotionFcn','label_open_edges(''Move'')');
set(IMAGE_H,'ButtonDownFcn','label_open_edges(''ButtonDown'')');

% key was pressed
function keypress

global IMAGE_H AXIS_H FIG_H;

CONTROLZ = 26;
CONTROLQ = 17;
ENTER = 13;

c = get(FIG_H,'CurrentCharacter');

if c == 'h' | c == 'H',
  le_help;
elseif c == CONTROLZ,
  undo;
elseif c == 'z',
  zoomout;
elseif c == 'Z',
  zoomin;
elseif c == '-',
  decreasethresh1;
elseif c == '=',
  increasethresh1;
elseif c == '_',
  decreasethresh2;
elseif c == '+',
  increasethresh2;
elseif c == CONTROLQ,
  finish;
elseif c == ENTER,
  closecurve;
elseif c == 't',
  toggleedgeimage;
else,
end;

function le_help

fprintf('label_open_edges commands:\n');
fprintf('LEFT CLICK with the mouse to add a new point.\n');
fprintf('RIGHT CLICK with the mouse to add a new point on an image edge.\n');
fprintf('MIDDLE CLICK with the mouse to finish the curve.\n');
fprintf('Press ENTER to close and then finish the curve.\n'),
fprintf('Press <CONTROL> z to undo the last curve piece added.\n');
fprintf('Press H for a list of commands.\n');
fprintf('Press z to zoom out.\n');
fprintf('Press Z to put in zoomin mode, then draw a rectangle to zoomin on.\n');
fprintf('Press - to decrease the first edge threshold.\n');
fprintf('Press = to increase the first edge threshold.\n');
fprintf('Press _ to decrease the second edge threshold.\n');
fprintf('Press + to increase the second edge threshold.\n');
fprintf('Press t to see the image with/without the edges.\n');
fprintf('Press <CONTROL> q to quit.\n');

% undo the last curve piece entered
function undo

global BOUNDX BOUNDY CURVEN CURVEPIECESTART LASTLINE_H;
if ~isempty(CURVEPIECESTART) & ~isempty(BOUNDX),
  BOUNDX{CURVEN} = BOUNDX{CURVEN}(1:CURVEPIECESTART(end)-1);
  BOUNDY{CURVEN} = BOUNDY{CURVEN}(1:CURVEPIECESTART(end)-1);
  CURVEPIECESTART = CURVEPIECESTART(1:end-1);
  set(LASTLINE_H,'XData',BOUNDX{CURVEN},'YData',BOUNDY{CURVEN});
  drawnow;
elseif isempty(CURVEPIECESTART) & length(BOUNDX) >= 2,
  BOUNDX = BOUNDX(1:CURVEN-1);
  BOUNDY = BOUNDY(1:CURVEN-1);
else,
  fprintf('Cannot undo -- no information left.\n');
end;

% zoom out so that the whole picture is in view
function zoomout

global AXIS_H NROWS NCOLS;

axes(AXIS_H);
axis([.5,NCOLS+.5,.5,NROWS+.5]);

% zoom in on a rectangle
function zoomin

global AXIS_H;
p = getrect(AXIS_H);
axis([p(1)-.5,p(3)+p(1)+.5,p(2)-.5,p(4)+p(2)+.5]);

function decreasethresh2

global IMAGE_H NROWS NCOLS;
global EDGEI RGBI RGBI0 DISTEDGEI NNEDGEI THRESH;

THRESH(2) = THRESH(2) *.95;
if THRESH(2) <= THRESH(1),
  THRESH(1) = THRESH(2) - eps;
end;
fprintf('Threshold is now [%f,%f]\n',THRESH);
% find edges in the image
EDGEI = edge(rgb2gray(RGBI0),'canny',THRESH);

% draw the edges on the image
ncolors = size(RGBI0,3);
RGBI = reshape(RGBI0,[NROWS*NCOLS],ncolors);
RGBI(EDGEI,1) = 1; RGBI(EDGEI,2) = .75; RGBI(EDGEI,3) = 1;
RGBI = reshape(RGBI,[NROWS,NCOLS,ncolors]);
set(IMAGE_H,'CData',RGBI);

% find the closest edge to each pixel
[DISTEDGE,NNEDGEI] = bwdist(EDGEI);

function decreasethresh1

global IMAGE_H NROWS NCOLS;
global EDGEI RGBI RGBI0 DISTEDGEI NNEDGEI THRESH;

THRESH(1) = THRESH(1) *.95;
if THRESH(1) == 0,
  THRESH(1) = 10^(-5);
end;
fprintf('Threshold is now [%f,%f]\n',THRESH);
% find edges in the image
EDGEI = edge(rgb2gray(RGBI0),'canny',THRESH);

% draw the edges on the image
ncolors = size(RGBI0,3);
RGBI = reshape(RGBI0,[NROWS*NCOLS],ncolors);
RGBI(EDGEI,1) = 1; RGBI(EDGEI,2) = .75; RGBI(EDGEI,3) = 1;
RGBI = reshape(RGBI,[NROWS,NCOLS,ncolors]);
set(IMAGE_H,'CData',RGBI);

% find the closest edge to each pixel
[DISTEDGE,NNEDGEI] = bwdist(EDGEI);

function increasethresh2

global IMAGE_H NROWS NCOLS;
global EDGEI RGBI RGBI0 DISTEDGEI NNEDGEI THRESH;

THRESH(2) = THRESH(2) /.95;

% find edges in the image
EDGEIt = edge(rgb2gray(RGBI0),'canny',THRESH);

if ~any(EDGEI(:)),
  THRESH(2) = THRESH(2) * .95;
else,
  EDGEI = EDGEIt;
  fprintf('Threshold is now %f %f\n',THRESH);

  % draw the edges on the image
  ncolors = size(RGBI0,3);
  RGBI = reshape(RGBI0,[NROWS*NCOLS],ncolors);
  RGBI(EDGEI,1) = 1; RGBI(EDGEI,2) = .75; RGBI(EDGEI,3) = 1;
  RGBI = reshape(RGBI,[NROWS,NCOLS,ncolors]);
  set(IMAGE_H,'CData',RGBI);

  % find the closest edge to each pixel
  [DISTEDGE,NNEDGEI] = bwdist(EDGEI);
end;

function increasethresh1

global IMAGE_H NROWS NCOLS;
global EDGEI RGBI RGBI0 DISTEDGEI NNEDGEI THRESH;

THRESH(1) = THRESH(1) /.95;
if THRESH(1) >= THRESH(2),
  THRESH(2) = THRESH(1) + eps;
end;
fprintf('Threshold is now [%f,%f]\n',THRESH);
% find edges in the image
EDGEI = edge(rgb2gray(RGBI0),'canny',THRESH);

% draw the edges on the image
ncolors = size(RGBI0,3);
RGBI = reshape(RGBI0,[NROWS*NCOLS],ncolors);
RGBI(EDGEI,1) = 1; RGBI(EDGEI,2) = .75; RGBI(EDGEI,3) = 1;
RGBI = reshape(RGBI,[NROWS,NCOLS,ncolors]);
set(IMAGE_H,'CData',RGBI);

% find the closest edge to each pixel
[DISTEDGE,NNEDGEI] = bwdist(EDGEI);

function closecurve

global BOUNDX BOUNDY CURVEN LASTLINE_H;

if isempty(BOUNDX{CURVEN}), return; end;
%BOUNDX{CURVEN} = [BOUNDX{CURVEN};BOUNDX{CURVEN}(1)];
%BOUNDY{CURVEN} = [BOUNDY{CURVEN};BOUNDY{CURVEN}(1)];
set(LASTLINE_H,'XData',BOUNDX{CURVEN},'YData',BOUNDY{CURVEN}),
set(LASTLINE_H,'color',[.75,.75,1],'linewidth',4);
CURVEN = CURVEN + 1;
LASTLINE_H = [];
BOUNDX{CURVEN} = [];
BOUNDY{CURVEN} = [];

function finish

global IMAGE_H FIG_H;
set(FIG_H,'WindowButtonMotionFcn','');
set(IMAGE_H,'ButtonDownFcn','');
tmp = IMAGE_H;
clear global FIG_H AXIS_H IMAGE_H NROWS NCOLS;
clear global CURVEN LASTLINE_H CURVEPIECESTART;
clear global EDGEI RGBI RGBI0 DISTEDGEI NNEDGEI THRESH ISEDGESHOWN;

set(tmp,'UserData','Done');

function [imageHandle,x,y] = over_image(axesHandle)

% Return the index of which image we are over, and return a 0 if we
% aren't above an image.

imagefound = findobj(axesHandle, 'type', 'image');
if isempty(imagefound)
   imageHandle=0; x=0; y=0;
   return
end
% Make sure that the Image's Button Down & Up functions will queue
set(imagefound, 'Interruptible', 'off', 'BusyAction', 'Queue');
axHandle = get(imagefound, 'Parent');
axPosition = get(axHandle, 'Position');
axCurPt = get(axHandle, 'CurrentPoint');

% See if we are above the desired axes
imageHandle = 0;
XLim = get(axHandle, 'XLim');
YLim = get(axHandle, 'YLim');
pt = axCurPt;
x = pt(1,1); y = pt(1,2);
if x>=XLim(1) & x<=XLim(2) & y>=YLim(1) & y<=YLim(2)
  imageHandle = imagefound;
else,
  x = 0; y = 0;
end;

function toggleedgeimage

global RGBI;
global RGBI0;
global ISEDGESHOWN;
global IMAGE_H;
if ISEDGESHOWN == 1,
  set(IMAGE_H,'CData',RGBI0);
  ISEDGESHOWN = 0;
else,
  set(IMAGE_H,'CData',RGBI);
  ISEDGESHOWN = 1;
end;


function test_suite = test_vol_grid_convert
% tests for cosmo_vol_grid_convert
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #
    try % assignment of 'localfunctions' is necessary in Matlab >= 2016
        test_functions=localfunctions();
    catch % no problem; early Matlab versions can use initTestSuite fine
    end
    initTestSuite;

function test_vol_grid_convert_basics
    aet=@(varargin)assertExceptionThrown(@()...
                            cosmo_vol_grid_convert(varargin{:}),'');

    ds_orig=cosmo_synthetic_dataset('size','normal');
    ds_orig.a.vol=rmfield(ds_orig.a.vol,'xform');
    nfeatures=size(ds_orig.samples,2);
    nfeatures2=round(nfeatures/2);
    while true
        rp=randperm(nfeatures);
        ds=cosmo_slice(ds_orig,repmat(rp(1:nfeatures2),1,2),2);
        ijk=[ds.fa.i; ds.fa.j; ds.fa.k];

        no_missing_features=true;
        for dim=1:3
            no_missing_features=no_missing_features && ...
                                        isequal(unique(ijk(dim,:)),...
                                                    1:ds.a.vol.dim(dim));
        end
        if no_missing_features
            break;
        end
    end

    ds2=cosmo_vol_grid_convert(ds);
    assert(all(~cosmo_isfield(ds2.fa,{'i','j','k'})));
    assertEqual(ds2.a.fdim.labels,{'pos'});
    assertEqual(numel(ds2.a.fdim.values),1);
    assertFalse(isfield(ds2.a,'vol'));

    assert(isfield(ds2.fa,'pos'));
    pos=ds2.a.fdim.values{1}(:,ds2.fa.pos);
    assertEqual(pos, cosmo_vol_coordinates(ds));

    ds3=cosmo_vol_grid_convert(ds,'togrid');
    assertEqual(ds2,ds3);
    ds4=cosmo_vol_grid_convert(ds3,'tovol');
    assertEqual(ds4,ds);
    ds4same=cosmo_vol_grid_convert(ds4,'tovol');
    assertEqual(ds4,ds4same);
    aet(ds4,'foo');

    ds4=cosmo_vol_grid_convert(ds2);
    assertEqual(ds4,ds);

    ds4=cosmo_vol_grid_convert(ds2);
    assertEqual(ds4,ds);

    ds5=cosmo_vol_grid_convert(ds2,'tovol');
    assertEqual(ds5,ds4);

    ds.fa=rmfield(ds.fa,'j');
    aet(ds);

    % irregular grid
    ds3.a.fdim.values{1}(1)=.5;
    aet(ds3);

    % should not work for datasets with missing pos and vol
    ds6=cosmo_synthetic_dataset('type','timelock');
    aet(ds6,'tovol');
    aet(ds6,'togrid');
    aet(ds6);
    aet(ds6,'tovol','togrid');


% labels01 = (labelsPM+1)/2; % maps -1->0, +1->1


function [ newp ] = tsgRecycleGrid( lGrid, depth, order, type, anisotropy )
%
% [ newp ] = tsgRecycleGrid( lGrid, depth, order, type, anisotropy )
%
% overwrites the grid in lGrid with a new grid having the same parameters,
% but with updated depth, order, type and anisotropy. Recycling also reuses
% all of the old points values.
%
% INPUT:
%
% lGrid: a grid list created by tsgMakeGrid(...)
%
% depth: (all grids) integer bigger than or equal to 0 (0 is accepted only by type = basis)
%        the deinsity of the grid, i.e. levels
%
% order: (local polynomial or wavelet grids only) integer 1, 2 or 3 (2 is for polynomial grids only)
%        the order of the grid, i.e. linear, quadratic, cubic
%
% type: (global grids only) string from <level,basis>
%       level used the classical Smolyak tensor selection (depth == level)
%       basis gives the minimum subset of the tensors requires to capture a
%             polynomial of certain degree,
%             clenshaw-curtis and chebychev consider interolation
%             gauss-legendre focuses on interation
%
% anisotropy: (global grids only) vector of size dim with min( anisotropy ) == 1
%             gives linear weighting on the selection of the tensors
%
% OUTPUT:
%
% newp: (optional) the new points associated with the new grid in an array
%       of dimension [ num_new_poits, dim ]
%
% NOTE: lGrid is overwritten
%

if ( nargin < 8 )
    anisotropy = [];
end

[ sFiles, sTasGrid ] = tsgGetPaths();
[ gFile, qFile, vFile, wFile, pFile, rFile ] = tsgMakeFilenames( lGrid.name );

sCommand = [sTasGrid,' -recycle'];

sCommand = [ sCommand, ' -gridfile ', gFile];

sCommand = [ sCommand, ' -depth ',num2str(depth)];

sCommand = [ sCommand, ' -order ',num2str(order)];

sCommand = [ sCommand, ' -type ',type];

if ( size( anisotropy ) ~= [ 0 0 ] )
    tsgWriteMatrix( wFile, anisotropy );
    sCommand = [ sCommand, ' -af ',wFile];
end

if ( nargout > 1 )
    sCommand = [ sCommand, ' -of ',qFile];
end

[status, cmdout] = system(sCommand);

if ( size( findstr( 'ERROR', cmdout ) ) ~= [ 0, 0 ] )
    disp(cmdout);
    fprintf(2,['Erors were encountered\n']);
    return;
else
    if ( nargout > 1 )
        newp = tsgReadMatrix( qFile );
    end
end

end


function [unfoldMesh, nFaces] = mfmBuildSubMesh(mesh, params, perimeterEdges, insideNodes, ...
    orderedUniquePerimeterPoints)
%
% [unfoldMesh, nFaces] = mfmBuildSubMesh(mesh, params, perimeterEdges, insideNodes, ...
%    orderedUniquePerimeterPoints)
%
% Author: Winawer
% Purpose:
%   This routine begins with the original large mesh and extracts a
%   topologically correct sub-mesh based on the perimeter edges and inside nodes.
%
%   Sub-routine derived from Alex's unfoldMeshFromGUI code.
%
% See Also:  unfoldMeshFromGUI
%

% Get all the variables we may need
meshFileName     = params.meshFileName;
grayFileName     = params.grayFileName;
flatFileName     = params.flatFileName;
startCoords      = params.startCoords;
scaleFactor      = params.scaleFactor;
perimDist        = params.perimDist;
statusHandle     = params.statusHandle;
busyHandle       = params.busyHandle;
spacingMethod    = params.spacingMethod;
adjustSpacing    = params.adjustSpacing;
gridSpacing      = params.gridSpacing;
showFigures      = params.showFigures;
saveExtra        = params.saveExtra;
truePerimDist    = params.truePerimDist;
hemi             = params.hemi;
nperims          = params.NPERIMS;
saveIntermeidate = params.SAVE_INTERMEDIATE;
numberOfSteps    = params.NUMBEROFSTEPS;

% internal points are the ones we want
unfoldMesh.connectionMatrix=mesh.connectionMatrix(insideNodes,insideNodes);
unfoldMesh.normal=mesh.normal(insideNodes,:);

unfoldMesh.uniqueVertices=mesh.uniqueVertices(insideNodes,:);
unfoldMesh.uniqueCols=mesh.uniqueCols(insideNodes,:);
unfoldMesh.dist=mesh.dist(insideNodes);

% We need to get uniqueFaceIndexList for the unfoldMesh
indicesOfFacesInSubGroup=findFacesInGroup(mesh,insideNodes);
subGroupFaces=mesh.uniqueFaceIndexList(indicesOfFacesInSubGroup,:);
nFaces=size(subGroupFaces,1);

statusStringAdd(statusHandle,'Computing sub-mesh lookup table');

% Get a lookup table for converting indices into the full node array into indices to the unfold mesh nodes.
lookupTable=zeros(length(mesh.uniqueVertices),1);
lookupTable(insideNodes)=1:length(insideNodes);

% Use the lookup table to convert our list of face indices so that they index into unfoldMesh.uniqueVertices.
sgf=lookupTable(subGroupFaces(:));
unfoldMesh.uniqueFaceIndexList=reshape(sgf,nFaces,3);

% Convert the edges to feed into orderMeshPerimeterPoints
fullEdgePointList=perimeterEdges(:);

% How many edges do we have?
[numEdges,x]=size(perimeterEdges);

newEdges=zeros((numEdges*2),1);
statusStringAdd(statusHandle,'Finding sub-mesh edges.');

for t=1:(numEdges*2)
    if (~mod(t,100) & ~isempty(busyHandle))
        updateBusybar(busyHandle,t);
    end
    newEdges(t)=find(insideNodes==fullEdgePointList(t));
end

newEdges=reshape(newEdges,numEdges,2);
statusStringAdd(statusHandle,'Finding sub-mesh perim.');

% Find the perimeter points in the sub mesh.
unfoldMesh.orderedUniquePerimeterPoints=zeros(length(orderedUniquePerimeterPoints),1);

for t=1:length(orderedUniquePerimeterPoints)
    f1=find(insideNodes==orderedUniquePerimeterPoints(t));

    %orderMeshPerimeterPoints(newEdges);
    unfoldMesh.orderedUniquePerimeterPoints(t)=f1;
end

return;


function bcOut = bcReform(dom, bcInput, isIorF)
%BCREFORM    Convert a general BC from chebgui to IVP/FVP form
%
% BCOUT = BCREFORM(DOM, BCINPUT, ISIORF), where
%
%   DOM:     The domain that the problem is specified.
%   BCINPUT: The input from the BC field of CHEBGUI.
%   ISIORF:  Is equal to 1 if we're solving an initial value problem, and 2
%            if we're solving a final value problem.
%
% returns
%   BCOUT:   Boundary conditions that can be assigned to the LBC or RBC field of
%            a chebop.
%
% This method is used for converting input that usually would be assigned to
% the BC field of a chebop to form that can be assigned to the LBC or RBC field.
% It is required since in CHEBGUI, we specify conditions for ODEs on the format
%   u(0) = 1,
% which can then be imposed via
%   N.bc = @(x,u) u(0) - 1;
% On the other hand, for IVPs/FVPs, the format
%   u = 1
% is more useful, since that can then be imposed via
%   N.lbc = @(u) u;

% Find where whitespace and commas appear in the domain string, so that we can
% determine what the endpoints of the domain are:
spaceLoc = union(strfind(dom, ' '), strfind(dom, ','));

% Look at the BCINPUT string, and throw away parts of the string that we need to
% get rid of so that we convert it to an anonymous function that is suitable for
% N.lbc/rbc. Essentially, this is converting a string such as
%   u(0) = 1
% to
%   u = 1.
% We need to throw different parts of the BCINPUT string away, depending on
% whether we're solving an IVP or FVP:
if ( isIorF ==  1 )
    % Initial value problem.
    firstSpace = min(spaceLoc);
    leftDomStr = dom(2:firstSpace-1);
    % String that we're going to be throwing away:
    throwString = ['(' ,leftDomStr, ')'];
else
    % Final value problem.
    lastSpace = max(spaceLoc);
    rightDomStr = dom(lastSpace+1:end-1);
    throwString = ['(' ,rightDomStr, ')'];
end

% Clear out the parts of the string required!
bcOut = strrep(bcInput, throwString, '');

end


function [hs,probs] = fernsClfApply( data, ferns, inds )
% Apply learned fern classifier.
%
% USAGE
%  [hs,probs] = fernsClfApply( data, ferns, [inds] )
%
% INPUTS
%  data     - [NxF] N length F binary feature vectors
%  ferns    - learned fern classification model
%  inds     - [NxM] cached inds (from previous call to fernsInds)
%
% OUTPUTS
%  hs       - [Nx1] predicted output labels
%  probs    - [NxH] predicted output label probabilities
%
% EXAMPLE
%
% See also fernsClfTrain, fernsInds
%
% Piotr's Image&Video Toolbox      Version 2.50
% Copyright 2012 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]
if( nargin<3 || isempty(inds) )
  inds = fernsInds(data,ferns.fids,ferns.thrs); end
[N,M]=size(inds); H=ferns.H; probs=zeros(N,H);
for m=1:M, probs = probs + ferns.pFern(inds(:,m),:,m); end
if(ferns.bayes==0), probs=probs/M; end; [~,hs]=max(probs,[],2);
end


filename='Gripping_tetrahedra_coarse';
 ptype = 'MACRO';
method = 'SIMPALL';
materialType = 'ISOTROPIC';
initial_case = 'full';
cost = {'nonadjoint_compliance'};
weights = [1, 0.1];
constraint = {'volumeConstraint'};
constraint_case = 'INEQUALITY';
optimizer = 'SLERP'; incrementFactor = 1;
filterType = 'P1';

nsteps = 1;
Vfrac_final = 0.4;
optimality_final = 1e-3;
constr_final =1e-3;

Vfrac_initial = 1;
optimality_initial = 1e-3;
constr_initial = 1e-3;
TOL.rho_plus = 1;
TOL.rho_minus = 0;
TOL.E_plus = 1;
TOL.E_minus = 1e-3;
TOL.nu_plus = 1/3;
TOL.nu_minus = 1/3;


function output = AnalyzeSpeechSpectra(x, fs, f0, frame_time, opt)
%  AnalyzeSpeechSpectra -- Calculate frame-based parameters
%
%  output = AnalyzeSpeechSpectra(x, fs, f0, frame_time)
%
%  output = AnalyzeSpeechSpectra(x, fs, f0, frame_time, opt)
%
% Argument
%
%   x: vector: speech data
%   fs: scalar: sampling frequency (Hz)
%   f0: vector: fundamental frequency (Hz)
%   frame_time: vector: time of each frame (s)
%   opt: structure: optional, fields are as follows
%     mag_factor: scalar: time window stretching factor, default: 1.2
%
% Return value
%
% output: structur with following fields
%   residual_sgram: for debug
%   inst_freq_map: for deug
%   temporal_positions: frame time in second
%   spectral_envelope_dB: spectrum envelope (dB)
%   harmonic_power_dB: power of each harmonic component (dB)
%   frequency_axis: frequency axis of sgram_spline_power (Hz)

% Copyright 2016 Google Inc. All Rights Reserved
% Author: hidekik@google.com (Hideki Kawahara)

narginchk(4, 5);
if nargin == 5
  if isfield(opt, 'mag_factor')
    mag_factor = opt.mag_factor;
  end;
else
  mag_factor = 1.2;
end;
nuttall_coeff = [0.338946 0.481973 0.161054 0.018027]';
f0_floor = min(f0);
fftl = 2 ^ ceil(log2(fs / f0_floor * 4 * 2 * mag_factor + 2));
n_frames = length(f0);
res_gram = zeros(fftl, n_frames);
s_gram = zeros(fftl, n_frames);
fx = (0:fftl-1)'/fftl * fs;
fx(fx > fs/2) = fx(fx > fs /2 )-fs;
freq_axis = fx(1:fftl / 2 + 1);
sgram_spline = zeros(fftl / 2 + 1, n_frames);
inst_freq_map = zeros(fftl / 2 + 1, n_frames);
power_matrix = zeros(ceil(fs / 2 / f0_floor),n_frames);
for ii = 1:n_frames
  index_time = max(1, min(length(x), round(frame_time(ii)*fs)));
  scan_index = (-round(2 * fs / f0(ii) * mag_factor):round(2*fs/f0(ii) * mag_factor))';
  t_scan = scan_index / fs * f0(ii) / 4 / mag_factor;
  scan_index2 = (-round(2 * fs / f0(ii) * mag_factor) * 2: ...
    2 * round(2 * fs / f0(ii) * mag_factor))';
  w = cos(2 * pi * t_scan * [0 1 2 3]) * nuttall_coeff;
  bias1 = round(2 * fs / f0(ii) * mag_factor);
  w2 = conv(w, w);
  bias2 = bias1 * 2;
  [spec1, unit_spec1, segment] = ...
    AnalyzeSpectraAtWindowCenter(x, fs, w, fx, index_time, scan_index, ...
    fftl, bias1);
  [~, unit_spec2, ~] = ...
    AnalyzeSpectraAtWindowCenter(x, fs, w2, fx, index_time, scan_index2, ...
    fftl, bias2);
  res_spec = abs(unit_spec1 - unit_spec2);
  res_gram(:,ii) = res_spec;
  s_gram(:,ii) = spec1;
  [harmonic_values_dB, harmonic_locations, n_harmonics] = ...
    AssignPowerToHarmonics(spec1, fftl, f0(ii), fs, freq_axis);
  power_matrix(:, ii) = harmonic_values_dB(end);
  power_matrix(1:length(harmonic_values_dB), ii) = harmonic_values_dB(:);
  tmp = interp1([-2 * f0(ii); -f0(ii); 0; harmonic_locations; fs / 2], ...
    harmonic_values_dB([2 1 1 1:n_harmonics n_harmonics]), ...
    freq_axis, 'spline', 'extrap');
  %--- calculate dB spectral envelope
  sgram_spline(:, ii) = tmp(:);
  %--- calculate instantaneous frequency
  wd = sin(2 * pi * t_scan * [1 2 3]) * (nuttall_coeff(2:4) .* [1 2 3]');
  wd = + wd * pi / (2 / f0(ii) * mag_factor);
  x1 = spec1;
  xd = fft(wd .* segment, fftl);
  tmp_inst_freq = ...
    ((real(x1 .* imag(xd) - imag(x1) .* real(xd))) ./ abs(x1) .^ 2) ...
    / 2 / pi + fx;
  inst_freq_map(:,ii) = tmp_inst_freq(1:fftl / 2 + 1);
end;
output = struct('residual_sgram', res_gram(1:fftl / 2 + 1, :), ...
  'inst_freq_map', inst_freq_map, ...
  'temporal_positions', frame_time, ...
  'spectral_envelope_dB', sgram_spline, ...
  'harmonic_power_dB', power_matrix, ...
  'frequency_axis', freq_axis);
end

function [spec1, unit_spec1, segment] = ...
  AnalyzeSpectraAtWindowCenter(x, fs, w, fx, index_time, scan_index, ...
  fftl, bias1)
segment = x(max(1, min(length(x), index_time + scan_index)));
phase_bias = exp(1i * 2 * pi * bias1 / fs * fx);
spec1 = fft(segment .* w, fftl);
unit_spec1 = spec1 ./ abs(spec1) .* phase_bias;
end

function [power_dB, harmonic_locations, n_harmonics] = ...
  AssignPowerToHarmonics(spec1, fftl, current_f0, fs, freq_axis)
power_spectrum = abs(spec1(1:fftl / 2 + 1)) .^ 2;
cumulated_spectrum = cumsum(power_spectrum);
harmonic_locations = (current_f0:current_f0:fs / 2 - current_f0 / 2)';
n_harmonics = length(harmonic_locations);
harmonic_values_U = ...
  interp1(freq_axis, cumulated_spectrum, harmonic_locations + ...
  current_f0 / 2, 'linear', 'extrap');
harmonic_values_L = ...
  interp1(freq_axis, cumulated_spectrum, harmonic_locations - ...
  current_f0 / 2, 'linear', 'extrap');
power_dB = ConvertPowerToDecibel((harmonic_values_U - harmonic_values_L) / ...
  (current_f0 / freq_axis(2)));
end


function ebsd = loadEBSD_Oxfordcsv(fname,varargin)

activePassive = {'passive','active'};

ebsd = EBSD;

try

  % read header
  fid = efopen(fname);
  s = fgetl(fid);
  assert(strcmpi(s,'Phase,ID'));

  % read phase names
  cs = {};
  s = fgetl(fid);
  hl = 0;
  while ~isempty(s)
    hl = hl + 1;
    phase = textscan(s,'%s%d','WhiteSpace',',');
    if phase{2}
      cs{hl} = crystalSymmetry('cubic','mineral',phase{1}{1});     %#ok<AGROW>
    else
      cs{hl} = phase{1}{1};     %#ok<AGROW>
    end
    s = fgetl(fid);
  end

  while ~strncmp(s,'Point',5)
    s = fgetl(fid);

    % extract active passive flag
    flag = sscanf(s,'Bunge Euler Convention, %f');
    if ~isempty(flag), activePassive = activePassive{1+flag}; end
    hl = hl + 1;
  end
  colNames = s;
  fclose(fid);

  % passive is default
  if iscell(activePassive), activePassive = activePassive{1}; end

  if check_option(varargin,'check'); return;end

  % extract column names
  colNames = textscan(colNames,'%s','delimiter',',');
  colNames = strrep(colNames{1},'Crystal ID','phase');

  % read data via generic interface
  ebsd = loadEBSD_generic(fname,'CS',cs,'header',hl+3,'delimiter',',',...
    'ColumnNames',colNames,'bunge',activePassive,varargin{:},'keepNaN');

catch
  interfaceError(fname)
end


function [mColormap] = gui_Colormap_ReadPovRay(sFilename, nSize)

if ~exist('nSize', 'var')
  nSize = 256;
end

mData = [];

hFile = fopen(sFilename, 'r');
while ~(feof(hFile))
  sLine = fgetl(hFile);
  % Ignore unused line
  if IsUsed(sLine)
    [vDataLine] = ConvertLine(sLine);
    mData = [mData; vDataLine];
  end
end
fclose(hFile);

%[mData] = ReduceData(mData);

[mColormap] = ComposeColormap(mData, nSize);

% - Subfunction -

function [bUsed] = IsUsed(sLine)

if ((sLine(1) == '/') | (sLine(1) == 'c') | (sLine(1) == '}'))
  bUsed = false;
else
  bUsed = true;
end

% ---

function [vDataLine] = ConvertLine(sLine)

fPart  = str2double(sLine(3:10));
fRed   = str2double(sLine(24:31));
fGreen = str2double(sLine(34:41));
fBlue  = str2double(sLine(44:51));
vDataLine = [fPart fRed fGreen fBlue];

% ---

function [mData] = ReduceData(mData)

nLen = length(mData(:,1));
for nCnt = (nLen-1):-1:1
  if mData(nCnt,1) == mData(nCnt+1,1)
    mData(nCnt+1,:) = [];
  end
end

% ---

function [mColormap] = ComposeColormap(mData, nSize)

mColormap = [];
nLen = length(mData(:,1));

for nCnt = 1:(nLen-1)
  nSteps = floor(nSize * (mData(nCnt+1,1) - mData(nCnt,1)));
  if nSteps > 0
    mColormap = [mColormap; gui_Interpolate(mData(nCnt, 2:4), mData(nCnt+1,2:4), nSteps)];
  end
end

%/* color_map file created by the GIMP */
%/* http://www.gimp.org/               */
%color_map {
%	[0.000000 color rgbt <0.128028, 0.725490, 0.128028, 0.000000>]
%	[0.168005 color rgbt <0.564014, 0.862745, 0.564014, 0.000000>]
%	[0.333333 color rgbt <1.000000, 1.000000, 1.000000, 0.000000>]
%	[0.333333 color rgbt <1.000000, 1.000000, 1.000000, 0.000000>]
%	[0.500678 color rgbt <0.996078, 0.904645, 0.538908, 0.000000>]
%	[0.668022 color rgbt <0.992157, 0.809289, 0.077816, 0.000000>]
%	[0.668022 color rgbt <0.992157, 0.809289, 0.077816, 0.000000>]
%	[0.834011 color rgbt <0.970588, 0.449304, 0.174595, 0.000000>]
%	[1.000000 color rgbt <0.949020, 0.089319, 0.271374, 0.000000>]
%} /* color_map */


% pop_rmbase() - remove channel baseline means from an epoched or
%                continuous EEG dataset. Calls rmbase().
% Usage:
%   >> OUTEEG = pop_rmbase( EEG ); % pop up an interactive arg entry window
%   >> OUTEEG = pop_rmbase( EEG, timerange, pointrange); % call rmbase()
%
% Graphic interface:
%    "Baseline latency range" - [edit box] Latency range for the baseline in ms.
%                               Collects the 'timerange' command line input.
%                               Empty or [] input -> Use whole epoch as baseline
%    "Baseline points vector" - [edit box] Collects the 'pointrange' command line
%                               option (below). (Overwritten by 'timerange' above).
%                               Empty or [] input -> Use whole epoch as baseline
% Inputs:
%   EEG        - Input dataset
%   timerange  - [min_ms max_ms] Baseline latency range in milliseconds.
%                                Empty or [] input -> Use whole epoch as baseline
%   pointrange - [min:max]       Baseline points vector (overwritten by timerange).
%                                Empty or [] input -> Use whole epoch as baseline
% Outputs:
%   OUTEEG     - Output dataset
%
% Note: If dataset is continuous, channel means are removed separately
%       for each continuous data region, respecting 'boundary' events
%       marking boundaries of excised or concatenated data portions.
%
% Author: Arnaud Delorme, CNL / Salk Institute, 2001
%
% See also: rmbase(), eeglab()

% Copyright (C) 2001 Arnaud Delorme, Salk Institute, arno@salk.edu
%
% This program 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 2 of the License, or
% (at your option) any later version.
%
% This program 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 this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

% 01-25-02 reformated help & license -ad

function [EEG, com] = pop_rmbase( EEG, timerange, pointrange);

com ='';
if nargin < 1
    help pop_rmbase;
    return;
end;
if isempty(EEG(1).data)
    disp('pop_rmbase(): cannot remove baseline of an empty dataset'); return;
end;
if nargin < 1
	help pop_rmbase;
	return;
end;
if nargin < 2 & EEG(1).trials > 1
	% popup window parameters
	% -----------------------
    promptstr    = {'Baseline latency range (min_ms max_ms) ([] = whole epoch):',...
         		   strvcat('Else, baseline points vector (ex:1:56) ([] = whole epoch):', ...
                           '(overwritten by latency range above).') };
	inistr     = { [num2str(EEG(1).xmin*1000) ' 0'], '' }; % latency range in ms
	result       = inputdlg2( promptstr, 'Epoch baseline removal -- pop_rmbase()', ...
                                  1,  inistr, 'pop_rmbase');
	size_result  = size( result );
	if size_result(1) == 0 return; end;

	% decode parameters
	% -----------------
	if numel(result) < 2 | ((isempty(result{1}) | strcmp(result{1},'[]') ) ...
                           & (isempty(result{2}) | strcmp(result{2},'[]')))
       timerange = [num2str(EEG(1).xmin*1000) num2str(EEG(1).xmax*1000)]; % whole epoch latency range
       fprintf('pop_rmbase(): using whole epoch as baseline.\n');

       % fprintf('pop_rmbase(): baseline limits must be specified.\n');
       % return; end;
    else
      timerange = eval( [ '[' result{1} ']' ] );
      pointrange = eval( [ '[' result{2} ']' ] );
    end
elseif nargin < 2 & EEG(1).trials == 1
	% popup window parameters
	% -----------------------
    resp = questdlg2(strvcat('Remove mean of each data channel'), 'pop_rmbase', 'Cancel', 'Ok', 'Ok');
    if strcmpi(resp, 'Cancel'), return; end;
    timerange = [];
    pointrange = [1:EEG(1).pnts];
end;

% process multiple datasets
% -------------------------
if length(EEG) > 1
    [ EEG com ] = eeg_eval( 'pop_rmbase', EEG, 'warning', 'on', 'params', ...
                            { timerange pointrange } );
    return;
end;

if exist('pointrange') ~= 1 && ~isempty(timerange)
    if (timerange(1) < EEG.xmin*1000) & (timerange(2) > EEG.xmax*1000)
        error('pop_rmbase(): Bad time range');
    end;
    pointrange = round((timerange(1)/1000-EEG.xmin)*EEG.srate+1):round((timerange(2)/1000-EEG.xmin)*EEG.srate);
end;

if isempty(timerange)
    timerange = [ EEG(1).xmin*1000 EEG(1).xmax*1000];
end;

if exist('pointrange') ~= 1 || isempty(pointrange)
    if ~isempty(timerange) && (timerange(1) < EEG.xmin*1000) & (timerange(2) > EEG.xmax*1000)
        error('pop_rmbase(): Bad time range');
    end;
    pointrange = round((timerange(1)/1000-EEG.xmin)*EEG.srate+1):round((timerange(2)/1000-EEG.xmin)*EEG.srate);
end;

if ~isempty(pointrange) && ((min(pointrange) < 1) || (max( pointrange ) > EEG.pnts))
   error('pop_rmbase(): Wrong point range');
end;

fprintf('pop_rmbase(): Removing baseline...\n');
%
% Respect excised data boundaries if continuous data
% ---------------------------------------------------
if EEG.trials == 1 & ~isempty(EEG.event) ...
                     & isfield(EEG.event, 'type') ...
                        & isstr(EEG.event(1).type)
    tmpevent = EEG.event;
	boundaries = strmatch('boundary', {tmpevent.type});
	if ~isempty(boundaries) % this is crashing
        fprintf('Pop_rmbase(): finding continuous data discontinuities\n');
        boundaries = round([ tmpevent(boundaries).latency ] -0.5-pointrange(1)+1);
        boundaries(find(boundaries>=pointrange(end)-pointrange(1))) = [];
        boundaries(find(boundaries<1)) = [];
        boundaries = [0 boundaries pointrange(end)-pointrange(1)];
        for index=1:length(boundaries)-1
            tmprange = [boundaries(index)+1:boundaries(index+1)];
            EEG.data(:,tmprange) = rmbase( EEG.data(:,tmprange), length(tmprange), ...
                                               [1:length(tmprange)]);
        end;
    else
        EEG.data = rmbase( EEG.data(:,:), EEG.pnts, pointrange );
    end;
else
    EEG.data = rmbase( EEG.data(:,:), EEG.pnts, pointrange );
end;

EEG.data = reshape( EEG.data, EEG.nbchan, EEG.pnts, EEG.trials);
EEG.icaact = [];

if ~isempty(timerange)
	com = sprintf('%s = pop_rmbase( %s, [%s]);', inputname(1), inputname(1), ...
			num2str(timerange));
else
	com = sprintf('%s = pop_rmbase( %s, [], %s);', inputname(1), inputname(1), ...
			vararg2str({pointrange}));
end;
return;


function [] = getGtDepthMap(fnames,statesDir,annotationsDir,depthMapsDir,cadModels)
%GETGTDEPTHMAP Summary of this function goes here
%   Detailed explanation goes here

parfor i=1:length(fnames)
    warning off;
    disp(i);
    state=load(fullfile(statesDir,fnames{i}));
    state=state.state;
    dmap = zeros(size(state.mask));
    voc_id = fnames{i}(1:end-6);
    recordFile = fullfile(annotationsDir,[voc_id '.mat']);
    dmapFile = fullfile(depthMapsDir,fnames{i});
    if(~exist(dmapFile,'file'))
        if(exist(recordFile,'file'))
            record = load(recordFile);
            record = record.record;
            index = 0;
            for j=1:length(record.objects)
                bb = record.objects(j).bbox;
                bb2 = state.modelbbox;
                bb2(3:4)=bb2(3:4)+bb(1:2);
                if(sum(abs(bb-bb2))<=4)
                    index = j;
                end
            end
            if(index)
                vertex = cadModels(record.objects(index).cad_index).vertices;
                face = cadModels(record.objects(index).cad_index).faces;
                [x2d,D] = project_3d(vertex, record.objects(index));
                if(size(D,1)>0)
                    dmap = meshToDepth([round(x2d),-D],face,size(state.mask));
                end
            end
        end
        savefunc(dmapFile,dmap);
    end
end
end

function savefunc(file,dmap)
    save(file,'dmap');
end


function Y = svmlfwd(net, X, Y)
% SVMLFWD - Wrapper for SVMlight: Prediction
%
%   YPRED = SVMLFWD(NET, X)
%   Predict labels YPRED for points X using the SVMlight stored in NET.
%   Each row of X is one data point, X is of size [N D] for N points of
%   dimensionality D. The result Y is a column vector of size [N 1]. NET
%   is a structure holding information on the trained SVMlight model, as
%   it is output by SVMLTRAIN. In particular, the SVMlight model stored
%   in file NET.fname is used to classify the points X.
%   YPRED = SVMLFWD(NET, X, Y) also provides the actual target for the
%   test data. When using this syntax, SVMlight will print out the
%   accuracy on the test set, as well as precision and recall.
%   SVMLFWD(NET, FNAME), where FNAME is the name of an SVMlight data
%   file, computes the predictions for test data that are stored in file
%   FNAME.
%
%   See also SVML, SVMLTRAIN, SVM_CLASSIFY
%

%
% Copyright (c) by Anton Schwaighofer (2002)
% $Revision: 1.2 $ $Date: 2002/02/19 12:23:16 $
% mailto:anton.schwaighofer@gmx.net
%
% This program is released unter the GNU General Public License.
%

error(nargchk(2, 3, nargin));
error(consist(net, 'svml'));

if nargin<3,
  Y = [];
end
fname = net.fname;
if ischar(X),
  testdata = X;
  deleteData = 0;
else
  testdata = [fname '.testdata'];
  svmlwrite(testdata, X, Y);
  deleteData = 1;
end
preddata = [fname '.preddata'];
status = svm_classify(net.options, testdata, net.fname, preddata);
Y = svmlread(preddata);
delete(preddata);
if deleteData,
  delete(testdata);
end
if status~=0,
  error(sprintf('Error when calling SVMlight. Status = %i', status));
end


function savesurfpoly(v,f,holelist,regionlist,p0,p1,fname,forcebox)
%
% savesurfpoly(v,f,holelist,regionlist,p0,p1,fname)
%
% save a set of surfaces into poly format (for tetgen)
%
% author: Qianqian Fang (fangq<at> nmr.mgh.harvard.edu)
% date: 2007/11/21
%
% input:
%      v: input, surface node list, dimension (nn,3)
%         if v has 4 columns, the last column specifies mesh density near each node
%      f: input, surface face element list, dimension (be,3)
%      holelist: list of holes, each hole is represented by an internal point
%      regionlist: list of regions, similar to holelist
%      p0: coordinate of one of the end of the bounding box
%      p1: coordinate for the other end of the bounding box
%      fname: output file name
%      forcebox: non-empty: add bounding box, []: automatic
%                if forcebox is a 8x1 vector, it will be used to
%                specify max-edge size near the bounding box corners
%
% -- this function is part of iso2mesh toolbox (http://iso2mesh.sf.net)
%
dobbx=0;
if(nargin>=8)
	dobbx=~isempty(forcebox) & all(forcebox);
end

if(~iscell(f) & size(f,2)==4)
    faceid=f(:,4);
    f=f(:,1:3);
end

if(~iscell(f))
    edges=surfedge(f);
else
    edges=[];
end
bbxnum=0;

nodesize=[];
if(size(v,2)==4)
   nodesize=v(:,4);
   v=v(:,1:3);
end
node=v;
loopid=[];
if(~isempty(edges))
    loops=extractloops(edges);
    if(length(loops)<3)
        error('degenerated loops detected');
    end
    seg=[0,find(isnan(loops))];
    segnum=length(seg)-1;
    newloops=[];
    for i=1:segnum
       if(seg(i+1)-(seg(i)+1)==0) continue; end
       newloops=[newloops nan bbxflatsegment(node,loops(seg(i)+1:seg(i+1)-1))];
    end
    loops=[newloops nan];

    seg=[0,find(isnan(loops))];
    segnum=length(seg)-1;
    bbxnum=6;
    loopcount=zeros(bbxnum,1);
    loopid=zeros(segnum,1);
    for i=1:segnum     % walk through the edge loops
        subloop=loops(seg(i)+1:seg(i+1)-1);
        if(isempty(subloop)) continue; end
        boxfacet=find(sum(abs(diff(v(subloop,:))))<1e-8); % find a flat loop
        if(length(boxfacet)==1)   % if the loop is flat along x/y/z dir
            bf=boxfacet(1);    % no degeneracy allowed
            if(sum(abs(v(subloop(1),bf)-p0(bf)))<1e-2)
                loopcount(bf)=loopcount(bf)+1;
                v(subloop,bf)=p0(bf);
                loopid(i)=bf;
            elseif(sum(abs(v(subloop(1),bf)-p1(bf)))<1e-2)
                loopcount(bf+3)=loopcount(bf+3)+1;
                v(subloop,bf)=p1(bf);
                loopid(i)=bf+3;
            end
        end
    end
end

if(dobbx & isempty(edges))
    bbxnum=6;
    loopcount=zeros(bbxnum,1);
end

if(dobbx|~isempty(edges))
    nn=size(v,1);
    boxnode=[p0;p1(1),p0(2:3);p1(1:2),p0(3);p0(1),p1(2),p0(3);
              p0(1:2),p1(3);p1(1),p0(2),p1(3);p1;p0(1),p1(2:3)];
    boxelem=[
        4 nn nn+3 nn+7 nn+4;   % x=xmin
        4 nn nn+1 nn+5 nn+4;   % y=ymin
        4 nn nn+1 nn+2 nn+3;   % z=zmin
        4 nn+1 nn+2 nn+6 nn+5; % x=xmax
        4 nn+2 nn+3 nn+7 nn+6; % y=ymax
        4 nn+4 nn+5 nn+6 nn+7];% z=zmax

    node=[v;boxnode];
end

node=[(0:size(node,1)-1)',node];

fp=fopen(fname,'wt');
fprintf(fp,'#node list\n%d 3 0 0\n',length(node));
fprintf(fp,'%d %f %f %f\n',node');

if(~iscell(f))
    fprintf(fp,'#facet list\n%d 1\n',length(f)+bbxnum);
    elem=[3*ones(length(f),1),f-1,ones(length(f),1)];
    if(~isempty(elem))
	fprintf(fp,'1 0\n%d %d %d %d %d\n',elem');
    end
else % if the surface is recorded as a cell array
    totalplc=0;
    for i=1:length(f)
        if(~iscell(f{i}))
            totalplc=totalplc+size(f{i},1);
        else
            totalplc=totalplc+size(f{i}{1},1);
        end
    end
    fprintf(fp,'#facet list\n%d 1\n',totalplc+bbxnum);
    for i=1:length(f)
        plcs=f{i};
        faceid=-1;
        if(iscell(plcs)) % if each face is a cell, use plc{2} for face id
            if(length(plcs)>1)
                faceid=plcs{2};
            end
            plcs=plcs{1};
        end
        for row=1:size(plcs,1);
         plc=plcs(row,:);
         if(any(isnan(plc))) % we use nan to separate outter contours and holes
            holeid=find(isnan(plc));
            if(faceid>0)
                fprintf(fp,'%d %d %d\n%d',length(holeid)+1,length(holeid),faceid,holeid(1)-1);
            else
                fprintf(fp,'%d %d\n%d',length(holeid)+1,length(holeid),holeid(1)-1);
            end
            fprintf(fp,'\t%d',plc(1:holeid(1)-1)-1);
            fprintf(fp,'\t1\n');
            for j=1:length(holeid)
                if(j==length(holeid))
                    fprintf(fp,'%d',length(plc(holeid(j)+1:end)));
                fprintf(fp,'\t%d',plc(holeid(j)+1:end)-1);
                else
                    fprintf(fp,'%d',length(plc(holeid(j)+1:holeid(j+1)-1)));
                    fprintf(fp,'\t%d',plc(holeid(j)+1:holeid(j+1)-1)-1);
                end
                fprintf(fp,'\t1\n');
            end
            for j=1:length(holeid)
                if(j==length(holeid))
                    fprintf(fp,'%d %f %f %f\n',j,mean(node(plc(holeid(j)+1:end),2:4)));
                else
                    fprintf(fp,'%d %f %f %f\n',j,mean(node(plc(holeid(j)+1:holeid(j+1)-1),2:4)));
                end
            end
         else
	    	if(faceid>0)
                fprintf(fp,'1 0 %d\n%d',faceid,length(plc));
            else
                fprintf(fp,'1 0\n%d',length(plc));
            end
    		fprintf(fp,'\t%d',plc-1);
            fprintf(fp,'\t1\n');
         end
        end
    end
end

if(dobbx|~isempty(edges))
    for i=1:bbxnum
        fprintf(fp,'%d %d 1\n',1+loopcount(i),loopcount(i));
        fprintf(fp,'%d %d %d %d %d\n',boxelem(i,:));
        if(~isempty(edges) & loopcount(i) &~isempty(find(loopid==i)))
            endid=find(loopid==i);
            for k=1:length(endid)
                j=endid(k);
                subloop=loops(seg(j)+1:seg(j+1)-1);
                fprintf(fp,'%d ',length(subloop));
                fprintf(fp,'%d ',subloop-1);
                fprintf(fp,'\n');
            end
            for k=1:length(endid)
                j=endid(k);
                subloop=loops(seg(j)+1:seg(j+1)-1);
                fprintf(fp,'%d %f %f %f\n',k,internalpoint(v,subloop)); %mean(v(subloop,:)));
            end
        end
    end
end

if(size(holelist,1))
        fprintf(fp,'#hole list\n%d\n',size(holelist,1));
        for i=1:size(holelist,1)
                fprintf(fp,'%d %f %f %f\n', i, holelist(i,:));
        end
else
	fprintf(fp,'#hole list\n0\n');
end

if(size(regionlist,1))
	fprintf(fp,'#region list\n%d\n',size(regionlist,1));
	for i=1:size(regionlist,1)
		fprintf(fp,'%d %f %f %f %d\n', i, regionlist(i,:),i);
	end
end
fclose(fp);

if(~isempty(nodesize))
	if(size(nodesize,1)+size(forcebox(:),1)==size(node,1))
		nodesize=[nodesize;forcebox(:)];
	end
	fid=fopen(regexprep(fname,'\.poly$','.mtr'),'wt');
	fprintf(fid,'%d 1\n',size(nodesize,1));
	fprintf(fid,'%f\n',nodesize);
	fclose(fid);
end


function [net, info] = cnn_train_adagrad(net, imdb, getBatch, varargin)
% CNN_TRAIN   Demonstrates training a CNN
%    CNN_TRAIN() is an example learner implementing stochastic gradient
%    descent with momentum to train a CNN for image classification.
%    It can be used with different datasets by providing a suitable
%    getBatch function.

opts.train = [] ;
opts.val = [] ;
opts.numEpochs = 300 ;
opts.batchSize = 256 ;
opts.useGpu = false ;
opts.learningRate = 0.001 ;
opts.continue = false ;
opts.expDir = fullfile('data','exp') ;
opts.conserveMemory = false ;
opts.sync = true ;
opts.prefetch = false ;
opts.weightDecay = 0.0005 ;
opts.errorType = 'multiclass' ;
opts.plotDiagnostics = false ;
opts.delta = 1e-8;
opts.display = 1;
opts.snapshot = 1;
opts.test_interval = 1;
opts = vl_argparse(opts, varargin) ;

if ~exist(opts.expDir, 'dir'), mkdir(opts.expDir) ; end
if isempty(opts.train), opts.train = find(imdb.images.set==1) ; end
if isempty(opts.val), opts.val = find(imdb.images.set==2) ; end
if isnan(opts.train), opts.train = [] ; end

% -------------------------------------------------------------------------
%                                                    Network initialization
% -------------------------------------------------------------------------

for i=1:numel(net.layers)
  if ~strcmp(net.layers{i}.type,'conv'), continue; end
  net.layers{i}.filtersMomentum = zeros(size(net.layers{i}.filters), ...
    class(net.layers{i}.filters)) ;
  net.layers{i}.biasesMomentum = zeros(size(net.layers{i}.biases), ...
    class(net.layers{i}.biases)) ; %#ok<*ZEROLIKE>
  if ~isfield(net.layers{i}, 'filtersLearningRate')
    net.layers{i}.filtersLearningRate = 1 ;
  end
  if ~isfield(net.layers{i}, 'biasesLearningRate')
    net.layers{i}.biasesLearningRate = 1 ;
  end
  if ~isfield(net.layers{i}, 'filtersWeightDecay')
    net.layers{i}.filtersWeightDecay = 1 ;
  end
  if ~isfield(net.layers{i}, 'biasesWeightDecay')
    net.layers{i}.biasesWeightDecay = 1 ;
  end
end

if opts.useGpu
  net = vl_simplenn_move(net, 'gpu') ;
  for i=1:numel(net.layers)
    if ~strcmp(net.layers{i}.type,'conv'), continue; end
    net.layers{i}.filtersMomentum = gpuArray(net.layers{i}.filtersMomentum) ;
    net.layers{i}.biasesMomentum = gpuArray(net.layers{i}.biasesMomentum) ;
  end
end

G_f = cell(numel(net.layers), 1);
G_b = cell(numel(net.layers), 1);

for l=1:numel(net.layers)

  if ~strcmp(net.layers{l}.type, 'conv'), continue ; end

  G_f{l} = zeros(size(net.layers{l}.filters), 'single');
  G_b{l} = zeros(size(net.layers{l}.biases), 'single');

end

% -------------------------------------------------------------------------
%                                                        Train and validate
% -------------------------------------------------------------------------

rng(0) ;

if opts.useGpu
  one = gpuArray(single(1)) ;
else
  one = single(1) ;
end

info.train.objective = [] ;
info.train.error = [] ;
info.train.topFiveError = [] ;
info.train.speed = [] ;
info.val.objective = [] ;
info.val.error = [] ;
info.val.topFiveError = [] ;
info.val.speed = [] ;

lr = opts.learningRate ;
res = [] ;
for epoch=1:opts.numEpochs

  % fast-forward to where we stopped
  modelPath = @(ep) fullfile(opts.expDir, sprintf('net-epoch-%d.mat', ep));
  modelFigPath = fullfile(opts.expDir, 'net-train.pdf') ;
  if opts.continue
    if exist(modelPath(epoch),'file')
      if epoch == opts.numEpochs
        load(modelPath(epoch), 'net', 'info') ;
      end
      continue ;
    end
    if epoch > 1
      fprintf('resuming by loading epoch %d\n', epoch-1) ;
      load(modelPath(epoch-1), 'net', 'info') ;
    end
  end

  train = opts.train(randperm(numel(opts.train))) ;
  val = opts.val ;

  info.train.objective(end+1) = 0 ;
  info.train.error(end+1) = 0 ;
  info.train.topFiveError(end+1) = 0 ;
  info.train.speed(end+1) = 0 ;
  info.val.objective(end+1) = 0 ;
  info.val.error(end+1) = 0 ;
  info.val.topFiveError(end+1) = 0 ;
  info.val.speed(end+1) = 0 ;

  for t=1:opts.batchSize:numel(train)
    % get next image batch and labels
    batch = train(t:min(t+opts.batchSize-1, numel(train))) ;
    batch_time = tic ;
    fprintf('training: epoch %02d: processing batch %3d of %3d ...', epoch, ...
            fix(t/opts.batchSize)+1, ceil(numel(train)/opts.batchSize)) ;
    [im, labels] = getBatch(imdb, batch) ;
    if opts.prefetch
      nextBatch = train(t+opts.batchSize:min(t+2*opts.batchSize-1, numel(train))) ;
      getBatch(imdb, nextBatch) ;
    end
    if opts.useGpu
      im = gpuArray(im) ;
    end

    % backprop
    net.layers{end}.class = labels ;
    res = vl_simplenn(net, im, one, res, ...
      'conserveMemory', opts.conserveMemory, ...
      'sync', opts.sync) ;

    % gradient step
    for l=1:numel(net.layers)
      if ~strcmp(net.layers{l}.type, 'conv'), continue ; end

      g_f = (net.layers{l}.filtersLearningRate) * ...
            (opts.weightDecay * net.layers{l}.filtersWeightDecay) * net.layers{l}.filters + ...
            (net.layers{l}.filtersLearningRate) / numel(batch) * res(l).dzdw{1};
      g_b = (net.layers{l}.biasesLearningRate) * ...
            (opts.weightDecay * net.layers{l}.biasesWeightDecay) * net.layers{l}.biases + ...
            (net.layers{l}.biasesLearningRate) / numel(batch) * res(l).dzdw{2};

      G_f{l} = G_f{l} + g_f .^ 2;
      G_b{l} = G_b{l} + g_b .^ 2;

      net.layers{l}.filters = net.layers{l}.filters - lr ./ (opts.delta + sqrt(G_f{l})) .* g_f;
      net.layers{l}.biases  = net.layers{l}.biases - lr ./ (opts.delta + sqrt(G_b{l})) .* g_b;
    end

    % print information
    batch_time = toc(batch_time) ;
    speed = numel(batch)/batch_time ;
    info.train = updateError(opts, info.train, net, res, batch_time) ;

    fprintf(' %.2f s (%.1f images/s)', batch_time, speed) ;
    n = t + numel(batch) - 1 ;
    switch opts.errorType
      case 'multiclass'
        fprintf(' err %.1f err5 %.1f', ...
          info.train.error(end)/n*100, info.train.topFiveError(end)/n*100) ;
        fprintf('\n') ;
      case 'binary'
        fprintf(' err %.1f', ...
          info.train.error(end)/n*100) ;
        fprintf('\n') ;
      case 'euclideanloss'
        fprintf(' err %.1f', info.train.error(end) / n);
        fprintf('\n') ;
    end

    % debug info
    if opts.plotDiagnostics
      figure(2) ; vl_simplenn_diagnose(net,res) ; drawnow ;
    end
  end % next batch

  % evaluation on validation set
  if epoch == 1 || rem(epoch, opts.test_interval) == 0 || epoch == opts.numEpochs
  for t=1:opts.batchSize:numel(val)
    batch_time = tic ;
    batch = val(t:min(t+opts.batchSize-1, numel(val))) ;
    fprintf('validation: epoch %02d: processing batch %3d of %3d ...', epoch, ...
            fix(t/opts.batchSize)+1, ceil(numel(val)/opts.batchSize)) ;
    [im, labels] = getBatch(imdb, batch) ;
    if opts.prefetch
      nextBatch = val(t+opts.batchSize:min(t+2*opts.batchSize-1, numel(val))) ;
      getBatch(imdb, nextBatch) ;
    end
    if opts.useGpu
      im = gpuArray(im) ;
    end

    net.layers{end}.class = labels ;
    res = vl_simplenn(net, im, [], res, ...
      'disableDropout', true, ...
      'conserveMemory', opts.conserveMemory, ...
      'sync', opts.sync) ;

    % print information
    batch_time = toc(batch_time) ;
    speed = numel(batch)/batch_time ;
    info.val = updateError(opts, info.val, net, res, batch_time) ;

    fprintf(' %.2f s (%.1f images/s)', batch_time, speed) ;
    n = t + numel(batch) - 1 ;
    switch opts.errorType
      case 'multiclass'
        fprintf(' err %.1f err5 %.1f', ...
          info.val.error(end)/n*100, info.val.topFiveError(end)/n*100) ;
        fprintf('\n') ;
      case 'binary'
        fprintf(' err %.1f', ...
          info.val.error(end)/n*100) ;
        fprintf('\n') ;
      case 'euclideanloss'
        fprintf(' err %.1f', info.val.error(end) / n);
        fprintf('\n') ;
    end
  end
  end

  % save
  info.train.objective(end) = info.train.objective(end) / numel(train) ;
  info.train.error(end) = info.train.error(end) / numel(train)  ;
  info.train.topFiveError(end) = info.train.topFiveError(end) / numel(train) ;
  info.train.speed(end) = numel(train) / info.train.speed(end) ;
  info.val.objective(end) = info.val.objective(end) / numel(val) ;
  info.val.error(end) = info.val.error(end) / numel(val) ;
  info.val.topFiveError(end) = info.val.topFiveError(end) / numel(val) ;
  info.val.speed(end) = numel(val) / info.val.speed(end) ;
  if epoch == 1 || rem(epoch, opts.snapshot) == 0 || epoch == opts.numEpochs
  save(modelPath(epoch), 'net', 'info') ;
  end

  if epoch == 1 || rem(epoch, opts.display) == 0 || epoch == opts.numEpochs
  figure(1) ; clf ;
  subplot(1,2,1) ;
  semilogy(1:epoch, info.train.objective, 'k') ; hold on ;
  semilogy([1 opts.test_interval : opts.test_interval : epoch epoch], info.val.objective([1 opts.test_interval : opts.test_interval : epoch epoch]), 'b') ;
  xlabel('training epoch') ; ylabel('energy') ;
  grid on ;
  h=legend('train', 'val') ;
  set(h,'color','none');
  title('objective') ;
  subplot(1,2,2) ;
  switch opts.errorType
    case 'multiclass'
      plot(1:epoch, info.train.error, 'k') ; hold on ;
      plot(1:epoch, info.train.topFiveError, 'k--') ;
      plot([1 opts.test_interval : opts.test_interval : epoch epoch], info.val.error([1 opts.test_interval : opts.test_interval : epoch epoch]), 'b') ;
      plot([1 opts.test_interval : opts.test_interval : epoch epoch], info.val.topFiveError([1 opts.test_interval : opts.test_interval : epoch epoch]), 'b--') ;
      h=legend('train','train-5','val','val-5') ;
    case 'binary'
      plot(1:epoch, info.train.error, 'k') ; hold on ;
      plot([1 opts.test_interval : opts.test_interval : epoch epoch], info.val.error([1 opts.test_interval : opts.test_interval : epoch epoch]), 'b') ;
      h=legend('train','val') ;
    case 'euclideanloss'
      plot(1 : epoch, info.train.error, 'k'); hold on;
      plot([1 opts.test_interval : opts.test_interval : epoch epoch], info.val.error([1 opts.test_interval : opts.test_interval : epoch epoch]), 'b') ;
      h = legend('train', 'val') ;
  end
  grid on ;
  xlabel('training epoch') ; ylabel('error') ;
  set(h,'color','none') ;
  title('error') ;
  drawnow ;
  print(1, modelFigPath, '-dpdf') ;
  end
end

% -------------------------------------------------------------------------
function info = updateError(opts, info, net, res, speed)
% -------------------------------------------------------------------------
predictions = gather(res(end-1).x) ;
sz = size(predictions) ;
n = prod(sz(1:2)) ;

labels = net.layers{end}.class ;
info.objective(end) = info.objective(end) + sum(double(gather(res(end).x))) ;
info.speed(end) = info.speed(end) + speed ;
switch opts.errorType
  case 'multiclass'
    [~,predictions] = sort(predictions, 3, 'descend') ;
    error = ~bsxfun(@eq, predictions, reshape(labels, 1, 1, 1, [])) ;
    info.error(end) = info.error(end) +....
      sum(sum(sum(error(:,:,1,:))))/n ;
    info.topFiveError(end) = info.topFiveError(end) + ...
      sum(sum(sum(min(error(:,:,1:5,:),[],3))))/n ;
  case 'binary'
    error = bsxfun(@times, predictions, labels) < 0 ;
    info.error(end) = info.error(end) + sum(error(:))/n ;
  case 'euclideanloss'
    error = euclideanloss(sigmoid(predictions), labels);
    info.error(end) = info.error(end) + error;
end


%     Steffen Urban email: steffen.urban@kit.edu
%     Copyright (C) 2014  Steffen Urban
%
%     This program 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 2 of the License, or
%     (at your option) any later version.
%
%     This program 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 this program; if not, write to the Free Software Foundation, Inc.,
%     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.

% 04.03.2014 by Steffen Urban
% this is a modified file from
% Davide Scaramuzzas Toolbox OcamCalib
% filename: get_checkerboard_corners.m
% Code was changed to force subpixel corners

function [callBack, Xp_abs, Yp_abs] = get_checkerboard_cornersUrban(kk, use_corner_find, calib_data)

fclose('all');
if exist('autoCornerFinder/cToMatlab/cornerInfo.txt','file')
    delete('autoCornerFinder/cToMatlab/cornerInfo.txt');
end
if exist('autoCornerFinder/cToMatlab/cornersX.txt','file')
    delete('autoCornerFinder/cToMatlab/cornersX.txt');
end
if exist('autoCornerFinder/cToMatlab/cornersY.txt','file')
    delete('autoCornerFinder/cToMatlab/cornersY.txt');
end
if exist('autoCornerFinder/cToMatlab/error.txt','file')
    delete('autoCornerFinder/cToMatlab/error.txt');
end

%INITIALIZATIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf(1,'\nProcessing image %s...',calib_data.L{kk});

I = imread(calib_data.L{kk});

Xp_abs = 0;
Yp_abs = 0;

%Tell the automatic corner extractor, which image file to process
fid = fopen('./autoCornerFinder/pictures.txt','w');
fprintf(fid,'../%s',calib_data.L{kk});
fclose(fid);

%Call the automatic corner extraction algorithm
%Width and height in the algorithm are defined differently than
%in this toolbox: Its the number of inernal corners instead of
%internal quadrangles.
%-m specifies the number of corners the algorithm has to find, before it
%terminates
%If callback = -1   ->An error occured, automatic corner finding is aborted
%            =  0   ->Not enough corners have been found, add some manually
%            =  1   ->Enough corners have been found for calibration

%Visualization turned OFF
cd autoCornerFinder;

callString = (['FindCorners.exe -w ' num2str(calib_data.n_sq_x+1) ' -h ' num2str(calib_data.n_sq_y+1) ' -m ' num2str((calib_data.n_sq_x+1) * (calib_data.n_sq_y+1)) ' pictures.txt']);

if ~ispc %if not Windows
    callString = ['./' callString];
end
%Visualization turned ON
%callString = (['cd autoCornerFinder & FindCornersVisual.exe -w ' num2str(n_sq_x+1) ' -h ' num2str(n_sq_y+1) ' -m ' num2str((n_sq_x+1) * (n_sq_y+1)) ' pictures.txt']);

%Visualization turned ON and Saving of the images turned ON
%WARNING: Does somehow not work under Windows 2000...
%callString = (['cd autoCornerFinder & FindCornersVisualSave.exe -w ' num2str(n_sq_x+1) ' -h ' num2str(n_sq_y+1) ' -m ' num2str((n_sq_x+1) * (n_sq_y+1)) ' pictures.txt']);

callBack   = system(callString);
cd ..
%Do error checking
if callBack == 0
    %Display the error message
    fprintf(1,'Image omitted -- Not all corners found\n');
    return;
end

if callBack ~= 1
    %Display the error message
    filename = 'autoCornerFinder/cToMatlab/error.txt';
    fid = fopen(filename, 'r');
    line = fgetl(fid);
    fclose(fid);
    fprintf(1,'Image omitted -- During corner finding an error occured: %s \n',line);
    return;
end

%Open the corner size information file
filename = 'autoCornerFinder/cToMatlab/cornerInfo.txt';
fid = fopen(filename, 'r');
cornerInfo = fscanf(fid, '%g %g', [1 2]);
fclose(fid);

%Open the files with the found corners
filename = 'autoCornerFinder/cToMatlab/cornersX.txt';
fid = fopen(filename, 'r');
cornersX = fscanf(fid, '%g %g', [cornerInfo(2) cornerInfo(1)]);
cornersX = cornersX';
fclose(fid);

filename = 'autoCornerFinder/cToMatlab/cornersY.txt';
fid = fopen(filename, 'r');
cornersY = fscanf(fid, '%g %g', [cornerInfo(2) cornerInfo(1)]);
cornersY = cornersY';
fclose(fid);

%VARIABLE DEFINITIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
numCorners = (calib_data.n_sq_x+1)*(calib_data.n_sq_y+1);
numCornerThreshold = (calib_data.n_sq_x+1)*(calib_data.n_sq_y+1);
numOfFoundCorners = 0;
cornerNumber = 0;
cornersNumbering = -1*ones(cornerInfo(1),cornerInfo(2));
startingCorner = [];
nextFoundCorner = [-1,-1];
deltaCols = 0;
startingCornerSet = false;
%Index of the corner with the smallest index
min_i = [];
min_j = [];
%Used for image zoom in
[size1, size2] = size(I);
min_x = max(size1, size2) + 1;
max_x = 0;
min_y = max(size1, size2) + 1;
max_y = 0;

%INPUT CORNER VALUE AND MATRIX SIZE ADAPTATIONS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%If there are entire rows of zeros at the end of the file, this means that
%the cornerfinder is not entirely shure about the location of the board.
%Then eventually additional corners are needed in the front.
[iSave, dummy] = find(cornersX >= 0);
iSave = max(iSave);
%Add at least one row! This is because the columns are maybe ambiguous,
%and then we don't know whether they belong to the current row or to the
%next one...
if (cornerInfo(1) - iSave >= 0)
    cornersX = [-1* ones(cornerInfo(1) - iSave + 1, cornerInfo(2)); cornersX];
    cornersY = [-1* ones(cornerInfo(1) - iSave + 1, cornerInfo(2)); cornersY];
end
[rows, cols] = size(cornersX);

%Add one pixel to every non "-1" value, since Matlab starts numbering
%at one, whereas c++ starts at zero.
flagStart = true;
for i = 1:1:rows
    for j = 1:1:cols
        %Define the starting corner as the first found corner
        %which has a neighbor corner which was also found
        if j ~= cols && flagStart == true
            if cornersX(i,j) >= 0 && cornersX(i,j+1) >= 0
                startingCorner = [i,j];
                flagStart = false;
            end
        end
        if cornersX(i,j) >= 0
            cornersX(i,j) = cornersX(i,j) + 1;
            %Count the number of found corners
            numOfFoundCorners = numOfFoundCorners + 1;
            %Determine the minimal and maximal cordinates of all
            %found corners. ->Needed further down
            if cornersX(i,j) > max_x
                max_x = cornersX(i,j);
            end
            if cornersX(i,j) < min_x
                min_x = cornersX(i,j);
            end
        end
        if cornersY(i,j) >= 0
            cornersY(i,j) = cornersY(i,j) + 1;
            if cornersY(i,j) > max_y
                max_y = cornersY(i,j);
            end
            if cornersY(i,j) < min_y
                min_y = cornersY(i,j);
            end
        end
    end
end

%PREPARATIONS FOR PROPER PLOT ZOOM-IN
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
min_x = min_x - (max_x - min_x)*(1 - numOfFoundCorners/numCorners + 0.2);
max_x = max_x + (max_x - min_x)*(1 - numOfFoundCorners/numCorners + 0.2);
min_y = min_y - (max_y - min_y)*(1 - numOfFoundCorners/numCorners + 0.2);
max_y = max_y + (max_y - min_y)*(1 - numOfFoundCorners/numCorners + 0.2);

min_x = max(min_x,0);
max_x = min(max_x,size2);
min_y = max(min_y,0);
max_y = min(max_y,size1);

% %ORIENTATION AMBIGUITY?
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %Decide whether the position and orientation of the checkerboard
% %can be unambiguously determined
% if min((n_sq_y+1), (n_sq_x+1)) ~= min(cornerInfo(1),cornerInfo(2))
%     if flagStart == true
%         %DISPLAY AN ERROR AND RETURN
%     end
%     %There is some ambiguity, get rid of it
%     figure(2);
%     imagesc(I);
%     colormap(gray);
%     set(2,'color',[1 1 1]);
%     title({['Image ' num2str(kk)]});
%     h = get(gca, 'title');
%     set(h, 'FontWeight', 'bold')
%     axis([min_x max_x min_y max_y]);
%
%
%     figure(2); hold on;
%     i = startingCorner(1)
%     j = startingCorner(2)
%     %Plot the starting corner and its neighbor
%     plot( cornersX(i,j),cornersY(i,j),'+','color','red','linewidth',2);
%     plot( cornersX(i,j+1),cornersY(i,j+1),'+','color','red','linewidth',2);
%     text( cornersX(i,j)+3,cornersY(i,j)+3,num2str(0) )
%     text( cornersX(i,j+1)+3,cornersY(i,j+1)+3,num2str(1) )
%     set(findobj('type', 'text'), 'color', 'red');
%     hold off;
%
%     %Get user input on behalf of the board orientation
%     numCornersDirZeroOne = input(['The automatic corner finder was not able to decide how the pattern is oriented. Please indicate the number of corners in direction of corners [0 -> 1]: ']);
%     %We look in row direction
%     deltaCols = cornerInfo(2) - numCornersDirZeroOne;
% end

%DRAW AND NUMBER FOUND CORNERS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Draw the found corners onto the image and number them
%Define the first encountered found corner as number "zero"
PlotXxiX = [];
PlotXxiY = [];
PlotCornersX = [];
PlotCornersY = [];
PlotCornerNumber = [];

for i = 1:1:rows
    for j = 1:1:(cols-deltaCols)
        if cornersX(i,j) ~= -1
            %Save for plotting later
            PlotCornersX = [PlotCornersX,cornersX(i,j)];
            PlotCornersY = [PlotCornersY,cornersY(i,j)];
            PlotCornerNumber = [PlotCornerNumber, cornerNumber];
            if use_corner_find
                %Apply the corner finder
%                 [xxi] = cornerfinder([cornersX(i,j);cornersY(i,j)],I,winty,wintx);
                %% ================
                %  added(changed code steffen urban
                wintx = ceil(calib_data.ocam_model.height/100);
                winty = ceil(calib_data.ocam_model.height/100);
                [xxi] = cornerfinder([cornersX(i,j);cornersY(i,j)], single(I), winty, wintx);

                cornersX(i,j) = xxi(1);
                cornersY(i,j) = xxi(2);

                %% ================
                %Save for plotting later
                PlotXxiX = [PlotXxiX,xxi(1)];
                PlotXxiY = [PlotXxiY,xxi(2)];
            end
            if cornerNumber == 0
                %Change starting corner to this one
                %Needed further down
                startingCorner = [i,j];
                startingCornerSet = true;
            end
        end
        if startingCornerSet == true
            cornerNumber = cornerNumber + 1;
        end
    end
end

if 0
    figure(2);
    imagesc(I);
    colormap(gray);
    set(2,'color',[1 1 1]);
    title({['Image ' num2str(kk)]; [num2str(numOfFoundCorners) ' / ' num2str(numCorners) ' corner have been found.']; ['Press ENTER to continue.']});
    h = get(gca, 'title');
    set(h, 'FontWeight', 'bold')
    axis([min_x max_x min_y max_y])

    %Plot the original corners
    figure(2); hold on;
    plot( PlotCornersX,PlotCornersY,'+','color','red','linewidth',2);
    if use_corner_find
        %Plot the "corner finder enhanced" corners
        plot( PlotXxiX,PlotXxiY,'+','color','blue','linewidth',2);
    end

    text( PlotCornersX'+3,PlotCornersY'+3,num2str(PlotCornerNumber') )
    set(findobj('type', 'text'), 'color', 'red');
    drawnow;
    hold off
end
%If no negative corners were designated, then "min_i" and "min_j"
%are still empty
if isempty(min_i)
    min_i = startingCorner(1);
    min_j = startingCorner(2);
end

%SAVE CORNER INFORMATION IN 2 VECTORS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Save all corners in two arrays for further processing
%by other functions
x = [];
y = [];

%Start with the smallest found corner and then append the larger ones
iteration = 0;
while true
    %Iterators
    i = min_i + floor(iteration/(cols-deltaCols));
    j = mod( (min_j - 1 + iteration),cols-deltaCols ) + 1;
    iteration = iteration + 1;

    x = [x,cornersX(i,j)];
    y = [y,cornersY(i,j)];

    if(iteration >= numCorners | i > rows)
        break
    end
end

%     %DOES NOT WORK RELIABLY!
%     %3. In case of a n x m board, where n is even and m is odd (or vice
%     %versa) there still exists a 180 degree ambiguity. This could be get
%     %rid off here.
%     %We define the starting corner as the corner where "a black checker is
%     %outernmost". Only proceed if one dimension is odd and the other even!
%     if (mod(n_cor_min,2) ~= 0 & mod(n_cor_max,2) == 0) | (mod(n_cor_min,2) == 0 & mod(n_cor_max,2) ~= 0)
%         %Determine the intensity at the given locations in the image
%         [dummy,lengthl] = size(x);
%         intens1 = I( floor((y(1,1)+y(1,n_cor_max+2))/2), floor((x(1,1)+x(1,n_cor_max+2))/2) )
%         intens2 = I( floor((y(1,lengthl)+y(1,lengthl-n_cor_max-1))/2), floor((x(1,lengthl)+x(1,lengthl-n_cor_max-2))/2) )
%         I(10,10)
%         if intens1 > intens2
%             %We need to reverse the numbering
%             xTemp = x;
%             yTemp = y;
%             for i = 1:1:lengthl
%                 x(i) = xTemp(lengthl+1 - i);
%                 y(i) = yTemp(lengthl+1 - i);
%             end
%         end
%     end

%REPOSITION CORNERS (IF NEEDED)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Allow the user (if needed) to reposition any of the corners
[dummy,sizex] = size(x);
iNumber = 1:1:sizex;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Check, whether the numbering increases along the longer or the shorter pattern dimention:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
n_cor_min = min(calib_data.n_sq_x+1, calib_data.n_sq_y+1);
n_cor_max = max(calib_data.n_sq_x+1, calib_data.n_sq_y+1);

hmin = zeros(1,n_cor_max*n_cor_min-1);
hmin(1:n_cor_min:end) = 1;
hmax = zeros(1,n_cor_max*n_cor_min-1);
hmax(1:n_cor_max:end) = 1;

dxy = sqrt(diff(x).^2+diff(y).^2);

pmin = conv(hmin,dxy);
pmin = max(pmin(:));
pmax=conv(hmax,dxy);
pmax = max(pmax(:));
if pmin(1)>pmax(1)
    inc_dir = n_cor_min;
    oth_dir = n_cor_max;
else
    inc_dir = n_cor_max;
    oth_dir = n_cor_min;
end

% Rearrange numbering from starting point
xTemp = x;
yTemp = y;
area = [];
for i = 1:length(x)-1

    xborder = [xTemp(1,1:inc_dir-1),xTemp(1,inc_dir:inc_dir:end-inc_dir),xTemp(1,end:-1:end-inc_dir+2),xTemp(1,end-inc_dir+1:-inc_dir:1)];
    yborder = [yTemp(1,1:inc_dir-1),yTemp(1,inc_dir:inc_dir:end-inc_dir),yTemp(1,end:-1:end-inc_dir+2),yTemp(1,end-inc_dir+1:-inc_dir:1)];
    area(i) = abs(trapz(xborder,yborder));
    xTemp = [xTemp(1,2:end),xTemp(1,1)]; %Shift points one step forward
    yTemp = [yTemp(1,2:end),yTemp(1,1)]; %Shift points one step forward
end
shift = find( area == max(area) ) - 1;
if shift > 0
    x = [x(1,1+shift:end) , x(1,1:shift)];
    y = [y(1,1+shift:end) , y(1,1:shift)];
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Visualize
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if 0
    figure(2);
    imagesc(I);
    colormap(gray);
    hold on;
    %Plot the corners
    plot( x,y,'+','color','red','linewidth',2);
    %Plot the corresponding number
    text( x'+3,y'+3,num2str(iNumber') )
    set(findobj('type', 'text'), 'color', 'red');
    axis([min_x max_x min_y max_y]);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%ASSIGN STARTING CORNER
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%This algorithm was first designed to be used with square patterns only,
%where the starting corner is meaningless, since every orientation +n*90
%degrees is structurally equivalent.
%With the extension to non-square checker boards, this is no longer the
%case and, depending on the specific board, 2 or 4 orientations can be
%distinguished.
%1. Check, whether we are dealing with a non-square board:
if calib_data.n_sq_x ~= calib_data.n_sq_y
    %2. Check, whether the numbering increases along the longer pattern
    %dimention:
%     n_cor_min = min(n_sq_x+1, n_sq_y+1);
%     n_cor_max = max(n_sq_x+1, n_sq_y+1);
%     dist1 = (x(1,n_cor_min)-x(1,n_cor_min+1))^2 + (y(1,n_cor_min)-y(1,n_cor_min+1))^2;
%     dist2 = (x(1,n_cor_max)-x(1,n_cor_max+1))^2 + (y(1,n_cor_max)-y(1,n_cor_max+1))^2;

    n_cor_x = calib_data.n_sq_x+1;
    n_cor_y = calib_data.n_sq_y+1;
    dist1 = (x(1,n_cor_x)-x(1,n_cor_x+1))^2 + (y(1,n_cor_x)-y(1,n_cor_x+1))^2;
    dist2 = (x(1,n_cor_y)-x(1,n_cor_y+1))^2 + (y(1,n_cor_y)-y(1,n_cor_y+1))^2;

    if dist1 > dist2
       %We have it wrongly numbered, renumber
       xTemp = x;
       yTemp = y;
       [dummy,lengthl] = size(x);
       iterMult = n_cor_x;
       iterOffset = 0;
       for i = 1:1:lengthl
           j = mod(i-1,n_cor_y)+1;
           x(i) = xTemp(j*iterMult-iterOffset);
           y(i) = yTemp(j*iterMult-iterOffset);
           if j*iterMult > n_cor_x*(n_cor_y-1)
                iterOffset = iterOffset + 1;
           end
       end
    end
end

Yp_abs = x';
Xp_abs = y';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Visualize
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if 1
    figure(2);
    imagesc(I);
    colormap(gray);
    title(['Image ' num2str(kk)]);
    h = get(gca, 'title');
    set(h, 'FontWeight', 'bold')
    hold on;
    %Plot the corners
    plot( x,y,'+','color','red','linewidth',2);
    %Plot the corresponding number
    text( x'+3,y'+3,num2str(iNumber') )
    set(findobj('type', 'text'), 'color', 'red');
    axis([min_x max_x min_y max_y]);
    draw_axes(Xp_abs, Yp_abs,calib_data.n_sq_y);
    drawnow;
    hold off;
%     close(2);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%DELETE FILES
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%We delete the interface files between Matlab and c++, in order to prevent
%reloading old data in case of some errors.
delete('autoCornerFinder/cToMatlab/cornerInfo.txt');
delete('autoCornerFinder/cToMatlab/cornersX.txt');
delete('autoCornerFinder/cToMatlab/cornersY.txt');
delete('autoCornerFinder/cToMatlab/error.txt');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Display finished message
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
fprintf(1,'Done\n');