function Bh = hmxMldivide(Mh,Bh)
%+========================================================================+
%|                                                                        |
%|         OPENHMX - LIBRARY FOR H-MATRIX COMPRESSION AND ALGEBRA         |
%|           openHmx is part of the GYPSILAB toolbox for Matlab           |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : hmxMldivide.m                                 |
%|    #    |   VERSION    : 0.51                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 14.03.2017                                    |
%|  / 0 \  |   LAST MODIF : 14.01.2019                                    |
%| ( === ) |   SYNOPSIS   : Solve Mh x = Bh with the rule                 |
%|  `---'  |                Compr > Full > H-Matrix                       |
%+========================================================================+

% Check dimensions
if (size(Mh,1) ~= size(Bh,1))
    error('hmxMldivide.m : matrix dimensions must agree.')
end

% Check input data
if ~isa(Mh,'hmx')
    error('hmxMldivide.m : unavailable case.')
end

%%% Lower H-Matrix \ H-Matrix -> H-Matrix
if islower(Mh) && isa(Bh,'hmx')
    % H-Matrix \ H-Matrix -> H-Matrix (recursion)
    if (Mh.typ == 0) && (Bh.typ == 0)
        % X11 -> L11 \ B11
        Bh.chd{1} = hmxMldivide(Mh.chd{1},Bh.chd{1});

        % X12 -> L11 \ B12
        Bh.chd{2} = hmxMldivide(Mh.chd{1},Bh.chd{2});

        % X21 -> L22 \ (B21 - L21*X11)
        Bh.chd{3} = Bh.chd{3} - Mh.chd{3} * Bh.chd{1};
        Bh.chd{3} = hmxMldivide(Mh.chd{4},Bh.chd{3});

        % X22 -> L22 \ (B22 - L21 * X12)
        Bh.chd{4} = Bh.chd{4} - Mh.chd{3} * Bh.chd{2};
        Bh.chd{4} = hmxMldivide(Mh.chd{4},Bh.chd{4});

        % Fusion
        Bh = hmxFusion(Bh);

    % H-Matrix \ Compr -> Compr
    elseif (Mh.typ == 0) && (Bh.typ == 1)
        Bh.dat = { hmxMldivide(Mh,Bh.dat{1}) , Bh.dat{2} };

    % H-Matrix \ Full -> Full
    elseif (Mh.typ == 0) && (Bh.typ == 2)
        Bh.typ = 2;
        Bh.dat = hmxMldivide(Mh,Bh.dat);

    % Compr \ --- -> ---
    elseif (Mh.typ == 1)
        error('hmxMldivide : unavailable case')

    % Full \ H-Matrix -> Unavailable
    elseif (Mh.typ == 2) && (Bh.typ == 0)
        error('hmxMldivide : unavailable case')

    % Full \ Compr -> Compr
    elseif (Mh.typ == 2) && (Bh.typ == 1)
        Bh.dat = { Mh.dat\Bh.dat{1} , Bh.dat{2} };

    % Full \ Full -> Full
    elseif (Bh.typ == 2)
        Bh.dat = Mh.dat \ Bh.dat;

    else
        error('hmxMldivide : unavailable case')
    end

%%% Upper H-Matrix \ H-Matrix -> H-Matrix
elseif isupper(Mh) && isa(Bh,'hmx')
    % H-Matrix \ H-Matrix -> H-Matrix (recursion)
    if (Mh.typ == 0) && (Bh.typ == 0)
        % X22 -> U22 \ B22
        Bh.chd{4} = hmxMldivide(Mh.chd{4},Bh.chd{4});

        % X21 -> U22 \ B21
        Bh.chd{3} = hmxMldivide(Mh.chd{4},Bh.chd{3});

        % X12 -> U11 \ (B12 - U12*X22)
        Bh.chd{2} = Bh.chd{2} - Mh.chd{2} * Bh.chd{4};
        Bh.chd{2} = hmxMldivide(Mh.chd{1},Bh.chd{2});

        % X11 -> U11 \ (B11 - U12 * X21)
        Bh.chd{1} = Bh.chd{1} - Mh.chd{2} * Bh.chd{3};
        Bh.chd{1} = hmxMldivide(Mh.chd{1},Bh.chd{1});

        % Fusion
        Bh = hmxFusion(Bh);

    % H-Matrix \ Compr -> Compr
    elseif (Mh.typ == 0) && (Bh.typ == 1)
        Bh.dat = { hmxMldivide(Mh,Bh.dat{1}) , Bh.dat{2} };

    % H-Matrix \ Full -> Full
    elseif (Mh.typ == 0) && (Bh.typ == 2)
        Bh.typ = 2;
        Bh.dat = hmxMldivide(Mh,Bh.dat);

    % Compr \ --- -> ---
    elseif (Mh.typ == 1)
        error('hmxMldivide : unavailable case')

    % Full \ H-Matrix -> Unavailable
    elseif (Mh.typ == 2) && (Bh.typ == 0)
        error('hmxMldivide : unavailable case')

    % Full \ Compr -> Compr
    elseif (Mh.typ == 2) && (Bh.typ == 1)
        Bh.dat = { Mh.dat\Bh.dat{1} , Bh.dat{2} };

    % Full \ Full -> Full
    elseif (Bh.typ == 2)
        Bh.dat = Mh.dat \ Bh.dat;

    else
        error('hmxMldivide : unavailable case')
    end

%%% Lower H-Matrix \ Matrix -> Matrix
elseif islower(Mh)
    if (size(Bh,2) > 0)
        % H-Matrix (recursion)
        if (Mh.typ == 0)
            % X1 -> L11 \ B1
            X1 = hmxMldivide(Mh.chd{1},Bh(Mh.row{1},:));

            % X2 -> L22 \ (B2 - L21*X1)
            X2 = Bh(Mh.row{3},:) - Mh.chd{3}*X1;
            X2 = hmxMldivide(Mh.chd{4},X2);

            % Bh = [X1 X2]
            if issparse(X1)
                Bh = sparse(size(Mh,1),size(Bh,2));
            else
                Bh = zeros(size(Mh,1),size(Bh,2),class(X1));
            end
            Bh(Mh.col{1},:) = X1;
            Bh(Mh.col{2},:) = X2;

        % Compressed leaf
        elseif (Mh.typ == 1)
            error('hmxMldivide : unavailable case')

        % Full leaf
        elseif (Mh.typ == 2)
            Bh = Mh.dat \ Bh;

        % Unknown type
        else
            error('hmxMldivide : unavailable case')
        end
    else
        Bh = zeros(size(Mh,1),0);
    end

%%% Upper H-Matrix \ Matrix -> Matrix
elseif isupper(Mh)
    if (size(Bh,2) > 0)
        % H-Matrix (recursion)
        if (Mh.typ == 0)
            % X2 -> U22 \ B2
            X2 = hmxMldivide(Mh.chd{4},Bh(Mh.row{4},:));

            % X1 -> U11 \ (B1 - U12*X2)
            X1 = Bh(Mh.row{1},:) - Mh.chd{2}*X2;
            X1 = hmxMldivide(Mh.chd{1},X1);

            % Bh = [X1 ; X2]
            if issparse(X1)
                Bh = sparse(size(Mh,1),size(Bh,2));
            else
                Bh = zeros(size(Mh,1),size(Bh,2),class(X1));
            end
            Bh(Mh.col{1},:) = X1;
            Bh(Mh.col{2},:) = X2;

        % Compressed leaf
        elseif (Mh.typ == 1)
            error('hmxMldivide : unavailable case')

        % Full leaf
        elseif (Mh.typ == 2)
            Bh = Mh.dat \ Bh;

        % Unknown type
        else
            error('hmxMldivide : unavailable case')
        end
    else
        Bh = zeros(size(Mh,1),0);
    end

%%% H-Matrix \ (H-)Matrix -> (H-)Matrix
else
    [Lh,Uh] = lu(Mh);
    Bh      = Uh\(Lh\Bh);
end


function model = yalmip2scs(interfacedata)

%% From internal
data.A = -interfacedata.F_struc(:,2:end);
data.b = full(interfacedata.F_struc(:,1));
data.c =  interfacedata.c;
cones = [];
cones.z = interfacedata.K.f; %scs renamed the cone from f to z
cones.l = interfacedata.K.l;
cones.q = interfacedata.K.q;
cones.s = interfacedata.K.s;
cones.ep =interfacedata.K.e;
param = interfacedata.options.scs;
param.verbose = interfacedata.options.verbose;

%% Extract lower diagonal form for new SCS format
if ~isempty(cones.s) && any(cones.s)
    sdpA = data.A(1+cones.l + cones.z+sum(cones.q):end,:);
    sdpb = data.b(1+cones.l + cones.z+sum(cones.q):end,:);
    expA = data.A(end-3*cones.ep+1:end,:);
    expb = data.b(end-3*cones.ep+1:end,:);
    data.A = data.A(1:cones.l + cones.z+sum(cones.q),:);
    data.b = data.b(1:cones.l + cones.z+sum(cones.q),:);
    top = 1;
    for i = 1:length(cones.s)
        A = sdpA(top:top + cones.s(i)^2-1,:);
        b = sdpb(top:top + cones.s(i)^2-1,:);
        n = cones.s(i);
        ind = find(speye(n));
        b(ind) = b(ind)/sqrt(2);
        A(ind,:) = A(ind,:)/sqrt(2);
        ind = find(tril(ones(n)));
        A = A(ind,:);
        b = b(ind);
        data.A = [data.A;A];
        data.b = [data.b;b];
        top = top  + cones.s(i)^2;
    end
    data.A = [data.A;expA];
    data.b = [data.b;expb];
end

%% Collect in one structure
model.data = data;
model.cones = cones;
model.param = param;


%
% The original code was downloaed from
% Ruslan Salakhutdinov\s hompeage:
%   http://http://www.mit.edu/~rsalakhu/
%
function ls = logdiff(xx,dim)
% ls = logsum(x,dim)
%
% returns the log of diff of logs
% similar to logsum.m function

if(length(xx(:))==1) ls=xx; return; end

xdims=size(xx);
if(nargin<2)
  dim=find(xdims>1);
end

alpha = max(xx,[],dim)-log(realmax)/2;
repdims=ones(size(xdims)); repdims(dim)=xdims(dim);
ls = alpha+log(diff(exp(xx-repmat(alpha,repdims)),dim));


function displayKaplanMeier_HN_tempFig5(pathRF,nameOutcomes,nameSets,thresholds)
% thresholds: (optional). Defines the thresholds of the risk stratification groups.
%             Defaut is one threshold of 0.5 for two risk groups.

if nargin < 4
    thresholds = 0.5;
else
    if numel(thresholds) > 2
        error('Max 3 curves (risk groups) are allowed for the moment')
    end
    thresholds = thresholds./100;
end
if numel(thresholds) == 1
    symbols = {'-b','--r'};
elseif numel(thresholds) == 2
    symbols = {'-g','--b','.-r'};
end

startpath = pwd;
nOutcomes = numel(nameOutcomes);

cd(pathRF), load('testing')
thresholds = [0,thresholds,1];
nCurves = numel(thresholds) - 1;
figure
for o = 1:nOutcomes
    legendCell = cell(1,nCurves);
    nameOutcome = nameOutcomes{o}; fSet = nameSets{o};
    if strcmp(nameOutcome,'Locoregional')
        name = 'Recurrence-free probability';
    elseif strcmp(nameOutcome,'Distant')
        name = 'Metastasis-free probability';
    elseif strcmp(nameOutcome,'Death')
        name = 'Survival probability';
    elseif strcmp(nameOutcome,'DeathSign')
        name = 'Survival probability';
    end
    load(['testResultsRF_',fSet,'_',nameOutcome]) % results gets out of there
    prob = results.probResponse;
    if strcmp(nameOutcome,'DeathSign')
        outcome = testing.outcomes.Death;
        timeData =  testing.timeToEvents.Death;
    else
        outcome = testing.outcomes.(nameOutcome);
        timeData =  testing.timeToEvents.(nameOutcome);
    end
    censData = 1 - outcome;
    hFig = subplot(1,nOutcomes,o); hold on, maxTT = 0; handles = []; tables1 = cell(1,nCurves); tables2 = cell(1,nCurves); X = cell(1,nCurves);
    for c = 1:nCurves
        time = timeData(prob >= thresholds(c) & prob < thresholds(c+1));
        cens = censData(prob >= thresholds(c) & prob < thresholds(c+1));
        [tableT,tableTT,t,T,xcg,ycg,~] = kmplot([time,cens],0.05,1,0); tables1{c} = tableT; tables2{c} = tableTT; X{c} = [time,cens];
        S = stairs(t,T,symbols{c},'LineWidth',2); handles = [handles,S];
        h = plot(xcg,ycg,'k+','MarkerSize',6,'LineWidth',1);
        maxT = max(t);
        if maxT > maxTT
            maxTT = maxT;
        end
        legendCell{c} = [num2str(thresholds(c)),' <= prob_{RF} < ',num2str(thresholds(c+1))];
    end
%     if nCurves == 2
%         pVal = getPval(tables1{1},tables1{2},tables2{1},tables2{2},X{1},X{2});
%         annotation(hFig,'textbox',...
%         [0.2635 0.300000001206285 0.22142856536167 0.0642857130794299],...
%         'String',{['p - value = ',num2str(pVal)]},...
%         'LineStyle','none','FitBoxToText','off');
%     elseif nCurves == 3
%         % Between low-risk and medium-risk curves (see bottom-left value)
%         pVal = getPval(tables1{1},tables1{2},tables2{1},tables2{2},X{1},X{2});
%         annotation(hFig,'textbox',...
%         [0.2635 0.300000001206285 0.22142856536167 0.0642857130794299],...
%         'String',{['p - value = ',num2str(pVal)]},...
%         'LineStyle','none','FitBoxToText','off');
%
%         % Between medium-risk and high-risk curves (see bottom-right value)
%         pVal = getPval(tables1{2},tables1{3},tables2{2},tables2{3},X{2},X{3});
%         annotation(hFig,'textbox',...
%         [0.6135 0.300000001206285 0.22142856536167 0.0642857130794299],...
%         'String',{['p - value = ',num2str(pVal)]},...
%         'LineStyle','none','FitBoxToText','off');
%     end
    axis([0 maxTT 0 1.05]), axis square
    ind = strfind(nameOutcome,'Death');
    if ~isempty(ind)
        nameOutcome(ind:ind+4) = [];
        nameOutcome = ['Survival',nameOutcome];
    end
    set(gca,'FontSize',12)
    title([nameOutcome,'\newline(',nameSets{o},')'],'FontSize',14)
    xlabel('Time (days)','FontSize',14)
    ylabel(name,'FontSize',14)
    if nCurves == 3
        legendCell = {'Low-risk group','Medium-risk group','High-risk group'};
    else
    end
    legend(handles,legendCell,'Location','SouthWest')
    grid ON
    hold off
end

cd(startpath)
end


% SP_PRECOMPUTE_PARAM: precompute all the fields, as in the space structure of the technical report, before mapping to the physical domain.
%
%     space = sp_precompute_param (space, msh);
%     space = sp_precompute_param (space, msh, 'option');
%
% INPUT:
%
%    space: object representing the discrete function space (see sp_scalar).
%    msh: mesh object containing the quadrature information (see msh_cartesian)
%   'option', value: additional optional parameters, currently available options are:
%
%              Name     |   Default value |  Meaning
%           ------------+-----------------+----------------------------------
%            value      |      true       |  compute shape_functions
%            gradient   |      false      |  compute shape_function_gradients
%            hessian    |      false      |  compute shape_function_hessians
%
% OUTPUT:
%
%    space: object containing the information of the input object, plus the
%            fields of the old structure, that are listed below. If no option
%            is given all the fields are computed. If an option is given,
%            only the selected fields will be computed.
%
%    FIELD_NAME      (SIZE)                             DESCRIPTION
%    ncomp           (scalar)                           number of components of the functions of the space (actually, 1)
%    ndof            (scalar)                           total number of degrees of freedom
%    ndof_dir        (1 x ndim vector)                  degrees of freedom along each direction
%    nsh_max         (scalar)                           maximum number of shape functions per element
%    nsh             (1 x msh.nel vector)               actual number of shape functions per each element
%    connectivity    (nsh_max x msh.nel vector)         indices of basis functions that do not vanish in each element
%    shape_functions (msh.nqn x nsh_max x msh.nel)      basis functions evaluated at each quadrature node in each element
%    shape_function_gradients
%          (ndim x msh.nqn x nsh_max x msh.nel)         basis function gradients evaluated at each quadrature node in each element
%    shape_function_hessians
%          (ndim x ndim x msh.nqn x nsh_max x msh.nel)  basis function hessians evaluated at each quadrature node in each element
%
% Copyright (C) 2009, 2010 Carlo de Falco
% Copyright (C) 2011, 2015, 2019 Rafael Vazquez
%
%    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 3 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, see <http://www.gnu.org/licenses/>.

function sp = sp_precompute_param (sp, msh, varargin)

  if (isempty (varargin))
    value = true;
    gradient = true;
    hessian = true;
  else
    if (~rem (length (varargin), 2) == 0)
      error ('sp_precompute: options must be passed in the [option, value] format');
    end
    value = false;
    gradient = false;
    hessian = false;
    for ii=1:2:length(varargin)-1
      if (strcmpi (varargin{ii}, 'value'))
        value = varargin{ii+1};
      elseif (strcmpi (varargin{ii}, 'gradient'))
        gradient = varargin{ii+1};
      elseif (strcmpi (varargin{ii}, 'hessian'))
        hessian = varargin{ii+1};
      else
        warning ('Ignoring unknown option %s', varargin {ii});
      end
    end
  end

  sp = struct (sp);

  sp_univ = sp.sp_univ;

  for idim = 1:msh.ndim
    elem_list{idim} = 1:msh.nel_dir(idim);
  end

  for idim = 1:msh.ndim
    nsh_dim{idim} = sp_univ(idim).nsh(elem_list{idim});
  end

  [nsh_grid{1:msh.ndim}] = ndgrid (nsh_dim{:});
  nsh = 1;
  for idim = 1:msh.ndim
    nsh = nsh .* nsh_grid{idim};
  end
  sp.nsh = nsh(:)';
  clear nsh

  for idim = 1:msh.ndim
    csize = ones (1, 2*msh.ndim);
    csize([idim, msh.ndim+idim]) = [sp_univ(idim).nsh_max, msh.nel_dir(idim)];
    crep = [sp_univ.nsh_max, msh.nel_dir];
    crep([idim, msh.ndim+idim]) = 1;

    conn{idim} = reshape (sp_univ(idim).connectivity(:,elem_list{idim}), csize);
    conn{idim} = repmat (conn{idim}, crep);
    conn{idim} = reshape (conn{idim}, [], msh.nel);
  end

  connectivity = zeros (sp.nsh_max, msh.nel);
  indices = ones (size (conn{1}));
  for idim = 1:msh.ndim
    indices = indices & conn{idim} ~= 0;
  end
  for idim = 1:msh.ndim
    conn{idim} = conn{idim}(indices);
  end
  connectivity(indices) = sub2ind ([sp.ndof_dir, 1], conn{:}); % The extra 1 makes things work in any dimension
  sp.connectivity = reshape (connectivity, sp.nsh_max, msh.nel);
  clear conn csize crep indices connectivity

  if (value || gradient || hessian)
    shp = cell(1,msh.ndim); shg = cell(1,msh.ndim); shh = cell(1,msh.ndim);
    for idim = 1:msh.ndim
      ssize = ones (1, 3*msh.ndim);
      ssize([idim, msh.ndim+idim, 2*msh.ndim+idim]) = [msh.nqn_dir(idim), sp_univ(idim).nsh_max, msh.nel_dir(idim)];
      srep = [msh.nqn_dir, sp_univ.nsh_max, msh.nel_dir];
      srep([idim, msh.ndim+idim, 2*msh.ndim+idim]) = 1;
      shp{idim} = reshape (sp_univ(idim).shape_functions(:,:,elem_list{idim}), ssize);
      shp{idim} = repmat (shp{idim}, srep);
      shp{idim} = reshape (shp{idim}, msh.nqn, sp.nsh_max, msh.nel);
      shg{idim} = reshape (sp_univ(idim).shape_function_gradients(:,:,elem_list{idim}), ssize);
      shg{idim} = repmat (shg{idim}, srep);
      shg{idim} = reshape (shg{idim}, msh.nqn, sp.nsh_max, msh.nel);
      shh{idim} = reshape (sp_univ(idim).shape_function_hessians(:,:,elem_list{idim}), ssize);
      shh{idim} = repmat (shh{idim}, srep);
      shh{idim} = reshape (shh{idim}, msh.nqn, sp.nsh_max, msh.nel);
    end

    if (value)
      sp.shape_functions = 1;
      for idim = 1:msh.ndim
        sp.shape_functions = sp.shape_functions .* shp{idim};
      end
    end

    if (gradient)
      for idim = 1:msh.ndim
        shape_fun_grad = shg{idim};
        for jdim = setdiff (1:msh.ndim, idim)
          shape_fun_grad = shape_fun_grad .* shp{jdim};
        end
        sp.shape_function_gradients(idim,:,:,:) = shape_fun_grad;
      end
      sp.shape_function_gradients = reshape (sp.shape_function_gradients, ...
                                    msh.ndim, msh.nqn, sp.nsh_max, msh.nel);
    end

% To be changed with isprop, as soon as classdef is working
    if (hessian && isfield (struct (msh), 'geo_map_der2'))
      for idim = 1:msh.ndim
        shape_fun_hess = shh{idim};
        for jdim = setdiff (1:msh.ndim, idim)
          shape_fun_hess = shape_fun_hess .* shp{jdim};
        end
        sp.shape_function_hessians(idim,idim,:,:,:) = shape_fun_hess;

        for jdim = setdiff (1:msh.ndim, idim)
          shape_fun_hess = shg{idim} .* shg{jdim};
          for kdim = setdiff (1:msh.ndim, [idim, jdim])
            shape_fun_hess = shape_fun_hess .* shp{kdim};
          end
          sp.shape_function_hessians(idim,jdim,:,:,:) = shape_fun_hess;
        end
      end
    end

    if (strcmpi (sp.space_type, 'NURBS'))
      sp = bsp_2_nrb__ (sp, msh, sp.weights);
    end
  end

end


function g = polyardKernGradient(kern, x, varargin)

% POLYARDKERNGRADIENT Gradient of POLYARD kernel's parameters.
% FORMAT
% DESC computes the gradient of functions with respect to the
% automatic relevance determination polynomial
% kernel's parameters. As well as the kernel structure and the
% input positions, the user provides a matrix PARTIAL which gives
% the partial derivatives of the function with respect to the
% relevant elements of the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x : the input locations for which the gradients are being
% computed.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The argument takes
% the form of a square matrix of dimension  numData, where numData is
% the number of rows in X.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters. The ordering of the vector should match
% that provided by the function kernExtractParam.
%
% FORMAT
% DESC computes the derivatives as above, but input locations are
% now provided in two matrices associated with rows and columns of
% the kernel matrix.
% ARG kern : the kernel structure for which the gradients are being
% computed.
% ARG x1 : the input locations associated with the rows of the
% kernel matrix.
% ARG x2 : the input locations associated with the columns of the
% kernel matrix.
% ARG partial : matrix of partial derivatives of the function of
% interest with respect to the kernel matrix. The matrix should
% have the same number of rows as X1 and the same number of columns
% as X2 has rows.
% RETURN g : gradients of the function of interest with respect to
% the kernel parameters.
%
% SEEALSO polyardKernParamInit, kernGradient, polyardKernDiagGradient, kernGradX
%
% COPYRIGHT : Neil D. Lawrence, 2005, 2006

% KERN

scales = sparse(diag(sqrt(kern.inputScales)));
xScale = x*scales;
if nargin < 4
  innerProd = xScale*xScale';
else
  xScale2 = varargin{1}*scales;
  innerProd = xScale*xScale2';
end
arg = kern.weightVariance*innerProd+kern.biasVariance;
base = kern.variance*kern.degree*arg.^(kern.degree-1);
baseCovGrad = base.*varargin{end};

g(1) = sum(sum(innerProd.*baseCovGrad));
g(2) = sum(sum(baseCovGrad));
g(3) = sum(sum(varargin{end}.*arg.^kern.degree));

if nargin < 4
  for j = 1:kern.inputDimension
    g(3+j) = sum(sum((x(:, j)*x(:, j)').*baseCovGrad))*kern.weightVariance;
  end
else
  for j = 1:kern.inputDimension
    g(3+j) = sum(sum((x(:, j)*varargin{1}(:, j)').*baseCovGrad))*kern.weightVariance;
  end
end


function L=Cell_watershed(I0, t4)
hy = fspecial('sobel');
hx = hy';
Iy = imfilter(double(I0), hy, 'replicate');
Ix = imfilter(double(I0), hx, 'replicate');
gradmag = sqrt(Ix.^2 + Iy.^2);
%figure, imshow(gradmag,[]), title('gradmag')

% first parameter
se = strel('disk', t4);
Io = imopen(I0, se);
%figure, imshow(Io), title('Io')

Ie = imerode(I0, se);
Iobr = imreconstruct(Ie, I0);
% figure, imshow(Iobr), title('Iobr')

Iobrd = imdilate(Iobr, se);
Iobrcbr = imreconstruct(imcomplement(Iobrd), imcomplement(Iobr));
Iobrcbr = imcomplement(Iobrcbr);
% figure, imshow(Iobrcbr), title('Iobrcbr')

fgm = imregionalmax(Iobrcbr,8);
%figure, imshow(fgm), title('fgm')

I2 = I0;
I2(fgm) = 255;
% figure, imshow(I2), title('fgm superimposed on original image')

% Second parameter
se2 = strel(ones(5,5));
fgm2 = imclose(fgm, se2);
fgm3 = imerode(fgm2, se2);

% Third parameter
fgm4 = bwareaopen(fgm3, 5);
I3 = I0;
I3(fgm4) = 255;
% figure, imshow(I3)
% title('fgm4 superimposed on original image')

bw = im2bw(Iobrcbr, graythresh(Iobrcbr));
%figure, imshow(bw), title('bw')
D = bwdist(bw);
DL = watershed(D);
bgm = DL == 0;
% figure, imshow(bgm), title('bgm')

gradmag2 = imimposemin(gradmag, bgm | fgm4);

L = watershed(gradmag2);
I4 = I0;
I4(imdilate(L == 0, ones(3, 3)) | bgm | fgm4) = 255;
% figure, imshow(I4)
% title('Markers and object boundaries superimposed on original image')

Lrgb = label2rgb(L, 'jet', 'w', 'shuffle');
% figure, imshow(Lrgb)
% title('Lrgb')
figure, imshow(I0), hold on
himage = imshow(Lrgb);
set(himage, 'AlphaData', 0.3);
title('nurse cell blobs')


function ar=lpczz2ar(zz)
%LPCZZ2AR Convert z-place poles to ar coefficients AR=(ZZ)
% The complex poles must occur in complex conjugate pairs
% but the order is unimportant.

%      Copyright (C) Mike Brookes 1997
%      Version: $Id: lpczz2ar.m,v 1.4 2007/05/04 07:01:39 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,p]=size(zz);
ar=zeros(nf,p+1);
for k=1:nf
  ar(k,:)=real(poly(zz(k,:)));
end


%% Load data
clear; close all; clc;

% 500-1000
% swf10 = load('swf_500_1000_10_dataset3.mat');
% swf50 = load('swf_500_1000_50_dataset3.mat');
% swf100 = load('swf_500_1000_100_dataset3.mat');
% msckf5 = load('msckf_500_1000_min5_maxInf');
% msckf10 = load('msckf_500_1000_min10_max50');
% msckf20 = load('msckf_500_1000_min20_max100');
% imu = load('imu_500_1000.mat');
% kStart = 500; kEnd = 1000;

% 1215-1715
swf10 = load('swf_1215_1715_10_dataset3.mat');
swf50 = load('swf_1215_1715_50_dataset3.mat');
swf100 = load('swf_1215_1715_100_dataset3.mat');
msckf5 = load('msckf_1215_1715_min5_maxInf');
msckf10 = load('msckf_1215_1715_min10_max50');
msckf20 = load('msckf_1215_1715_min20_max100');
imu = load('imu_1215_1715.mat');
kStart = 1215; kEnd = 1715;

%% Compute RMSE
swf10_trans_rmse = sqrt(mean(swf10.swf_trans_err.^2,1));
swf10_rot_rmse = sqrt(mean(swf10.swf_rot_err.^2,1));

swf50_trans_rmse = sqrt(mean(swf50.swf_trans_err.^2,1));
swf50_rot_rmse = sqrt(mean(swf50.swf_rot_err.^2,1));

swf100_trans_rmse = sqrt(mean(swf100.swf_trans_err.^2,1));
swf100_rot_rmse = sqrt(mean(swf100.swf_rot_err.^2,1));

msckf5_trans_rmse = sqrt(mean(msckf5.msckf_trans_err.^2,1));
msckf5_rot_rmse = sqrt(mean(msckf5.msckf_rot_err.^2,1));

msckf10_trans_rmse = sqrt(mean(msckf10.msckf_trans_err.^2,1));
msckf10_rot_rmse = sqrt(mean(msckf10.msckf_rot_err.^2,1));

msckf20_trans_rmse = sqrt(mean(msckf20.msckf_trans_err.^2,1));
msckf20_rot_rmse = sqrt(mean(msckf20.msckf_rot_err.^2,1));

imu_trans_rmse = sqrt(mean(imu.msckf_trans_err.^2,1));
imu_rot_rmse = sqrt(mean(imu.msckf_rot_err.^2,1));

%% Plot stuff
figure(1); clf;
fontSize = 14;
lineWidth = 2;
pos = [200,200,640,400];
xLim = [kStart,kEnd];
k = kStart:kEnd;
xticks = linspace(kStart,kEnd,11);

set(gcf, 'Position', pos);

% Translational RMSE
subplot(2,1,1);
plot(k,imu_trans_rmse, '-k', 'LineWidth', lineWidth); hold on;
plot(k,msckf5_trans_rmse, '-b', 'LineWidth', lineWidth);
plot(k,msckf10_trans_rmse, '--b', 'LineWidth', lineWidth);
plot(k,msckf20_trans_rmse, '-.b', 'LineWidth', lineWidth);
plot(k,swf10_trans_rmse, '-g', 'LineWidth', lineWidth);
plot(k,swf50_trans_rmse, '--g', 'LineWidth', lineWidth);
plot(k,swf100_trans_rmse, '-.g', 'LineWidth', lineWidth);
xlim(xLim);
set(gca,'XTick',xticks);
legend('IMU Only','MSCKF 5-Inf', 'MSCKF 10-50', 'MSCKF 20-100', 'SWF 10', 'SWF 50', 'SWF 100','Location', 'northwest');
title('Window Size Comparison');
ylabel('Trans. RMSE (m)')
set(gca,'FontSize',fontSize)
grid minor; box on;

% Rotational RMSE
subplot(2,1,2);
plot(k,imu_rot_rmse, '-k', 'LineWidth', lineWidth); hold on;
plot(k,msckf5_rot_rmse, '-b', 'LineWidth', lineWidth);
plot(k,msckf10_rot_rmse, '--b', 'LineWidth', lineWidth);
plot(k,msckf20_rot_rmse, '-.b', 'LineWidth', lineWidth);
plot(k,swf10_rot_rmse, '-g', 'LineWidth', lineWidth);
plot(k,swf50_rot_rmse, '--g', 'LineWidth', lineWidth);
plot(k,swf100_rot_rmse, '-.g', 'LineWidth', lineWidth);
xlim(xLim);
set(gca,'XTick',xticks);
legend('IMU Only','MSCKF 5-Inf', 'MSCKF 10-50', 'MSCKF 20-100', 'SWF 10', 'SWF 50', 'SWF 100','Location', 'northwest');
ylabel('Rot. RMSE (Axis-Angle)');
xlabel('Timestep');
set(gca,'FontSize',fontSize)
grid minor; box on;

%% Export figure
fileName = sprintf('RMSE-Comparison-WindowSize-%d-%d.pdf',kStart,kEnd);
export_fig(gcf, fileName, '-transparent');


function backDiffUy = backdiffy(u)
% backward diff in y direction in equation (2.9a)
% \Delta^y_{+}u_{ij} = u_{i,j+1} - u_{ij}

cols = size(u,2);
% u([2:rows rows], :) is u_{i+1,j} in (2.9a)
backDiffUy = u(:, [2:cols cols]) - u;

github-repos/MATLAB/YimianDai-Image-Processing-Codes-for-Easier-Understanding/Image-Processing-Codes-for-Easier-Understanding-874302799e48852624bc3760b58b46bd9360f238/src/(Physica D 1992) Nonlinear Total Variation based noise removal algorithms/version_1/support/backdiffy.m


function varargout=mod(varargin)
%MOD (overloaded)

switch class(varargin{1})

    case 'sdpvar' % Overloaded operator for SDPVAR objects. Pass on args and save them.

        if ~isa(varargin{2},'double')
            error('MOD is currently only supported for DOUBLE second argument');
        end

        x = varargin{1};
        y = varargin{2};
        % Some boring code for scalarization
        if prod(size(x))==1 & prod(size(y))>1
            x = x*ones(size(y));
        elseif prod(size(y))==1 & prod(size(x))>1
            y = y*ones(size(x));
        end
        if ~all(size(x) == size(y))
            error('Matrix dimensions must agree.');
        end
        dim = size(x);
        x = reshape(x,prod(dim),1);
        y = reshape(y,prod(dim),1);
        z = [];
        % Create one variable for each element
        for i = 1:length(x)
            xi = extsubsref(x,i);
            yi = extsubsref(y,i);
            inarg = {xi,yi};
            z = [z;yalmip('define',mfilename,inarg{:})];
        end
        z = reshape(z,dim);
        varargout{1} = z;

    case 'char' % YALMIP send 'graph' when it wants the epigraph or hypograph

        % Description using epigraphs
        t = varargin{2};
        x = varargin{3};
        y = varargin{4};

        % t = mod(x,y), i.e. t = x - n*y, n = floor(x/y)
        n = intvar(1,1);
        F = (t == x - y*n)  + ((x/y) - 1 <= n <= (x/y));

        varargout{1} = F;
        varargout{2} = struct('convexity','none','monotonicity','none','definiteness','none','model','integer');
        varargout{3} = [x(:);y(:)];

    otherwise
        error('Strange type on first argument in SDPVAR/MOD');
end


classdef SO3KernelHandle < SO3Kernel
  % defines a kernel function as a function of the rotational angle

  properties
    fun = @(x) 1;
  end

  methods

    function psi = SO3KernelHandle(fun,varargin)

      % extract parameter and halfwidth
      if nargin == 0, return;end

      psi.fun = fun;
      psi.A = calcFourier(psi,getMTEXpref('maxS2Bandwidth'),varargin{:});

    end

    function c = char(psi)
      c = ['custom, halfwidth ' ...
        xnum2str(psi.halfwidth/degree) mtexdegchar];
    end

    function value = eval(psi,co2)
      % the kernel function on SO(3)
      co2 = cut2unitI(co2);
      value   =  psi.fun(co2);
    end

  end
end


%compute head speed in the direction perpendicular to the central-head direction
function [data,units]=compute_velheadperch(trx,n)

larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
velheadperch=cell(1,numlarvae);
centralheadangperp=cell(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
    centralheadangperp{1,i}=trx(larva).centralheadang-pi/2;
    velheadperch{1,i}=trx(larva).velmaghead.*(cos(trx(larva).velanghead).*cos(trx(larva).centralheadang(1,1:end-1)+pi/2)+sin(trx(larva).velanghead).*sin(trx(larva).centralheadang(1,1:end-1)+pi/2));
end

units=parseunits('mm/s');
data=velheadperch;


function solution = MP_GWO(UAV, SearchAgents, Max_iter)
%MP_GWO 多种群灰狼优化算法
%Multi Population Gray Wolf Optimization

% 超参数
g = 50;       % 动态更新加权系数

% 算法初始化
[WolfPops, ~] = PopsInit(UAV, SearchAgents, false);   % 随机生成 初始狼群
ClassPops = PopsCluster(WolfPops, UAV);                    % 进行初始聚类（用来获得 k 值）
dim = WolfPops.PosDim;                                                 % 状态变量维度
cSearchAgents = ClassPops.SearchAgents;                    % 搜索智能体个数（子种群数量）
SearchAgents = cSearchAgents * ClassPops.k;              % 对所有智能体个数进行修正（k的整数倍）
WolfPops.Pos = WolfPops.Pos(1:SearchAgents, :);       % 对种群进行修正

% 报错
if cSearchAgents < 4
    error('搜索智能体个数过少')
end

% 初始化解
Alpha_pos = zeros(ClassPops.k, dim);          % α解
Alpha_score = 1 ./ zeros(ClassPops.k, 1);     % α解适应度

Beta_pos = zeros(ClassPops.k, dim);            % β解
Beta_score = 1 ./ zeros(ClassPops.k, 1);       % β解适应度

Delta_pos = zeros(ClassPops.k, dim);           % δ解
Delta_score = 1 ./ zeros(ClassPops.k, 1);      % δ解适应度

Fitness_list = zeros(ClassPops.k, Max_iter);  % 适应度曲线

Pops.PosDim = WolfPops.PosDim;  % 子种群
Pops.lb = WolfPops.lb;
Pops.ub = WolfPops.ub;

% 迭代求解
tic
fprintf('>>MP-GWO 优化中    00.00%%')
for iter = 1 : Max_iter

    % ①  更新参数a
    a = 2 - iter * 2 / Max_iter;               % 线性递减
    %a = 2 * cos((iter/Max_iter)*pi/2);   % 非线性递减

    % ②  聚类
    if iter > 1 %初始化时聚完一次了，节省计算量
    ClassPops = PopsCluster(WolfPops, UAV);
    end
    for k = 1 : ClassPops.k
        Positions = ClassPops.Pos{k};

        % ③  寻找 α、β、δ 狼
        for i = 1 : cSearchAgents
            % 读取目标函数
            fitness = ClassPops.Fitness(k, i);

            % 更新 Alpha、Beta 和 Delta 解
            if fitness <= Alpha_score(k)  % 适应能力最强（因为性能指标越小越好，因此为小于号）
                Alpha_score(k) = fitness;
                Alpha_pos(k, :) = Positions(i, :);
            end
            if fitness > Alpha_score(k) && fitness <= Beta_score(k)
                Beta_score(k) = fitness;
                Beta_pos(k, :) = Positions(i, :);
            end
            if fitness > Alpha_score(k) && fitness > Beta_score(k) && fitness <= Delta_score(k)
                Delta_score(k) = fitness;
                Delta_pos(k, :) = Positions(i, :);
            end
        end

        % ④  更新位置（朝着前三只狼位置前进）
        for i = 1 : cSearchAgents
            for j = 1 : dim

                r1 = rand();
                r2 = rand();
                A1 = 2*a*r1 - a;
                C1 = 2*r2;
                D_alpha = abs(C1*Alpha_pos(k, j) - Positions(i, j));
                X1 = Alpha_pos(k, j) - A1*D_alpha;

                r1 = rand();
                r2 = rand();
                A2 = 2*a*r1 - a;
                C2 = 2*r2;
                D_beta = abs(C2*Beta_pos(k, j) - Positions(i, j));
                X2 = Beta_pos(k, j) - A2*D_beta;

                r1 = rand();
                r2 = rand();
                A3 = 2*a*r1 - a;
                C3 = 2*r2;
                D_delta = abs(C3*Delta_pos(k, j) - Positions(i, j));
                X3 = Delta_pos(k, j) - A3*D_delta;

                % 静态更新
                Positions(i, j) = (X1 + X2 + X3) / 3;
                % 动态更新
                %                 q = g * a; %阈值
                %                 if abs(Alpha_score(k)-Delta_score(k)) > q
                %                     Sum_score = Alpha_score(k) + Beta_score(k) + Delta_score(k);
                %                     Positions(i, j) = (Alpha_score(k)*X1 + Beta_score(k)*X2 + Delta_score(k)*X3) / Sum_score;
                %                 else
                %                     Positions(i, j) = (X1 + X2 + X3) / 3;
                %                 end

            end
        end

    % ⑤  调整不符合要求的状态变量
    Pops.Pos = Positions;
    ProbPoints =  ClassPops.ProbPoints{k};
    [Pops, ~] = BoundAdjust(Pops, ProbPoints, UAV);

    % ⑥  存储适应度
    Fitness_list(k, iter) = Alpha_score(k);

    % ⑦  合并种群
    WolfPops.Pos(cSearchAgents*(k-1)+1:cSearchAgents*k, :) = Pops.Pos;

    end

    if iter/Max_iter*100 < 10
        fprintf('\b\b\b\b\b%.2f%%', iter/Max_iter*100)
    else
        fprintf('\b\b\b\b\b\b%.2f%%', iter/Max_iter*100)
    end
end
fprintf('\n\n>>计算完成！\n\n')
toc

% 寻找 α β δ 位置
n = 3;
A = ClassPops.Fitness;
t = findmin(A, n);
index = cSearchAgents * (t(:, 1) - 1) + t(:, 2);

real_Alpha_no = index(1);
real_Beta_no = index(2);
real_Delta_no = index(3);
Alpha_Data = ClassPops.Data{t(1, 1)}{t(1, 2)}  ;

% 输出值
solution.method = 'MP-GWO';                                 % 算法
% solution.ClassPops = ClassPops;                               % 分类信息
solution.WolfPops = WolfPops;                                % 所有解种群信息
solution.Tracks = Pops2Tracks(WolfPops, UAV);    % 所有解航迹信息
solution.Fitness_list = mean(Fitness_list, 1);            % 所有α解的平均适应度曲线
solution.Alpha_Data = Alpha_Data;                          % 真 · α 的威胁信息
solution.Alpha_no = real_Alpha_no;                          % 真 · α 的位置
solution.Beta_no = real_Beta_no;                               % 真 · β 的位置
solution.Delta_no = real_Delta_no;                            % 真 · δ 的位置

end

%% 寻找A矩阵中最小n个数的位置
function t = findmin(A, n)
    t = sort(A(:));
    [x, y] = find(A <= t(n), n);
    t = [x, y];         % 前n个最小项在矩阵A中的位置[行,列]
    B = zeros(n, 1);
    for i = 1 : n
        B(i) = A(t(i, 1), t(i, 2));
    end
    [~, index] = sort(B);
    t = t(index, :); % 前n个从小到大排序的位置
end

%% 对种群进行聚类
function [ClassPops] = PopsCluster(WolfPops, UAV)

SearchAgents = size(WolfPops.Pos, 1);  % 智能体个数
Dim = WolfPops.PosDim;                        % 智能体维度
Tracks = Pops2Tracks(WolfPops, UAV); % 智能体转换成航迹信息

% 计算适应度
o_Fitness = zeros(SearchAgents, 1); % 60*1
o_subF = []; % 5*60
o_ProbPoints = cell(SearchAgents, 1);  % 60*1
o_Data = cell(SearchAgents, 1); % 60*1

% parfor 并行计算适应度
for i = 1:SearchAgents
    [fitness, subF, Data] = ObjFun(Tracks{i}, UAV);
    o_ProbPoints{i} = Data.ProbPoint;  %cell-cell
    o_Data(i) = {Data}; %cell-struct
    o_Fitness(i) = fitness; %vector-var
    o_subF = [o_subF, subF];
end

% 分类
k = size(subF, 1);                                              % 分 k 类（由objfun决定）
cSearchAgents = floor(SearchAgents / k);    % 并行智能体个数
cFitness = zeros(k, cSearchAgents);               % 保存每类的适应度
cPositions = cell(k, 1);                                     % 保存每类的位置信息
cTracks = cell(k, 1);                                          % 保存每类的航迹信息
cProbPoints = cell(k, 1);                                  % 保存每类的有问题航迹点
cData = cell(k, 1);                                             % 存储每类的检测报告

% 排序
[~, Index] = sort(o_subF, 2, "ascend") ;     % 沿维度2升序排序，返回新矩阵和序号
                                                                       % fitness越小越好
% 聚类
for i = 1:k
    Positions = zeros(cSearchAgents, Dim);
    batchTrack = cell(cSearchAgents, 1);
    batchProbPoints = cell(cSearchAgents, 1);
    batchData = cell(cSearchAgents, 1);
    for j = 1:cSearchAgents
        idx = Index(i, j);
        cFitness(i, j) = o_Fitness(idx); %mat-vector
        Positions(j, :) = WolfPops.Pos(idx, :); %mat-mat
        batchTrack{j} = Tracks{idx}; %cell-cell
        batchProbPoints{j} = o_ProbPoints{idx}; %cell-cell
        batchData{j} = o_Data{idx}; %cell-cell
    end
    cPositions(i) = {Positions}; %cell-mat
    cTracks{i} = batchTrack; %cell-cell
    cProbPoints{i} = batchProbPoints; %cell-cell
    cData{i} = batchData; %cell-cell
end

% 输出
ClassPops.Pos = cPositions;
ClassPops.Tracks = cTracks;
ClassPops.ProbPoints = cProbPoints;
ClassPops.Data = cData;
ClassPops.Fitness = cFitness;
ClassPops.SearchAgents = cSearchAgents;
ClassPops.k = k;
end


function [h,parvar,Q,v] = sosd(X,ops)
%SOSD Returns sum-of-squares decomposition
%
% h = sosd(p)
%
% Example
%
%  x = sdpvar(1,1);sdisplay(sosd(x^4 + 1))

if isfield(X.extra,'sosid')
    % This is used internally when calling sosd on SOS constraint
    [h,parvar,Q,v] = yalmip('getsos',X.extra.sosid);
else
    F = sos(X);
    if nargin == 1
        solvesos(F,[],sdpsettings('verbose',0));
    else
        solvesos(F,[],ops);
    end
    [h,parvar,Q,v] = sosd(F);
end


function w_omega = rotationalPID_velocity(w_R_b,w_R_b_des,w_omega_des,Kp)

    % ROTATIONALPID_VELOCITY implements a controller trajectory tracking inside
    %                        the group SO(3). The angular velocity is assumed
    %                        to be a control input.
    %
    % FORMAT: w_omega = rotationalPID_velocity(w_R_b,w_R_b_des,w_omega_des,Kp)
    %
    % INPUT: - w_R_b       = [3 * 3] rotation matrix
    %        - w_R_b_des   = [3 * 3] desired rotation matrix
    %        - w_omega     = [3 * 1] angular velocity (expressed in the world frame)
    %        - w_omega_des = [3 * 1] desired angular velocity (expressed in the world frame)
    %        - Kp          = [3 * 3] orientation gains
    %
    % OUTPUT: - w_omega    = [3 * 1] input angular velocity
    %
    % Authors: Daniele Pucci, Marie Charbonneau, Gabriele Nava
    %
    %          all authors are with the Italian Istitute of Technology (IIT)
    %          email: name.surname@iit.it
    %
    % Genoa, Dec 2017
    %

    %% --- Initialization ---

    skv      = wbc.skewVee(w_R_b*transpose(w_R_b_des));
    w_omega  = w_omega_des -Kp*skv;
end


classdef TestEM
    %TestEM

    properties (Constant)
        X = [randn(10,3)+1; randn(10,3)-1; randn(10,3)-2];
    end

    methods (Static)
        function test_clustering
            [N,d] = size(TestEM.X);
            C = 3;
            model = cv.EM();
            model.ClustersNumber = C;
            model.CovarianceMatrixType = 'Diagonal';
            model.TermCriteria.maxCount = 100;
            model.TermCriteria.epsilon = 1e-6;

            model.train(TestEM.X);
            assert(model.isTrained());

            [LL, IDX, probs] = model.trainEM(TestEM.X);
            validateattributes(LL, {'numeric'}, {'vector', 'numel',N});
            validateattributes(IDX, {'numeric'}, ...
                {'vector', 'integer', '<',C, 'numel',N});
            validateattributes(probs, {'numeric'}, ...
                {'size',[N C], 'real', '>=',0, '<=',1});

            weights = model.getWeights();
            validateattributes(weights, {'numeric'}, {'vector', 'numel',C});

            means = model.getMeans();
            validateattributes(means, {'numeric'}, {'size',[C d]});

            covs = model.getCovs();
            validateattributes(covs, {'cell'}, {'vector'});
            if ~mexopencv.isOctave()
                %HACK: https://savannah.gnu.org/bugs/index.php?46328
                cellfun(@(covar) validateattributes(covar, {'numeric'}, ...
                    {'square', 'diag', 'size',[d d]}), covs);
            end

            [LL, IDX, probs] = model.trainE(TestEM.X, means);
            [LL, IDX, probs] = model.trainE(TestEM.X, means, 'Covs0',covs);
            [LL, IDX, probs] = model.trainE(TestEM.X, means, ...
                'Covs0',covs, 'Weights0',weights);

            [LL, IDX, probs] = model.trainM(TestEM.X, probs);

            probs = model.predict(TestEM.X);
            validateattributes(probs, {'numeric'}, ...
                {'size',[N C], 'real', '>=',0, '<=',1});

            [LL, IDX, probs] = model.predict2(TestEM.X);
            validateattributes(LL, {'numeric'}, {'vector', 'numel',N});
            validateattributes(IDX, {'numeric'}, ...
                {'vector', 'integer', '<',C, 'numel',N});
            validateattributes(probs, {'numeric'}, ...
                {'size',[N C], 'real', '>=',0, '<=',1});
        end

        function test_data_options
            model = cv.EM();
            N = size(TestEM.X, 1);
            model.train(TestEM.X, 'Data',{'Layout','Row', 'VarType','NNN', ...
                'VarIdx',[], 'SampleIdx',[], 'SampleWeights',ones(N,1), ...
                'TrainTestSplitRatio',1/3, 'TrainTestSplitShuffle',true});
        end

        function test_storage
            fname = tempname();
            model = cv.EM();
            model.train(TestEM.X);

            model.save([fname '.xml']);
            cleanObj = onCleanup(@() delete([fname '.xml']));
            model1 = cv.EM();
            model1.load([fname '.xml']);
            %isequal(model, model1)

            model.save([fname '.yaml']);
            cleanObj = onCleanup(@() delete([fname '.yaml']));
            model2 = cv.EM([fname '.yaml']);
            %isequal(model, model2)

            model1.clear();
            model2.clear();
        end

        function test_serialization
            model = cv.EM();
            model.train(TestEM.X);
            strXML = model.save('.xml');
            strYML = model.save('.yml');

            model2 = cv.EM();
            model2.load(strXML, 'FromString',true);
            Yhat = model2.predict(TestEM.X);

            model3 = cv.EM(strYML, 'FromString',true);
            Yhat = model3.predict(TestEM.X);
        end
    end

end


function [ fea, out ] = ex_magnetostatics5( varargin )
%EX_MAGNETOSTATICS5 Magnetic field test model.
%
%   [ FEA, OUT ] = EX_MAGNETOSTATICS5( VARARGIN ) Magnetic field test model.
%
%   Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       sfun        string {sflag2}        Shape function for pressure
%       hmax        scalar {0.1}           Grid size
%       iplot       scalar 0/{1}           Plot solution (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { 'sfun',     'sflag2';
            'hmax',     0.1;
            'iplot',    1;
            'tol',      1e-2;
            'fid',      1 };
[got,opt] = parseopt(cOptDef,varargin{:});
fid       = opt.fid;

% Geometry and grid generation.
fea.sdim = { 'x' 'y' };
fea.grid = rectgrid( round(1/opt.hmax) );
fea.grid.s( selcells(fea,'y<=(0.5+sqrt(eps))') ) = 2;

% Problem definition.
fea = addphys( fea, @magnetostatics );
fea.phys.ms.eqn.coef{3,end} = { 1 0 };
fea.phys.ms.eqn.coef{4,end} = { 0 1 };
fea.phys.ms.sfun = { opt.sfun };

% Parse and solve problem.
fea       = parsephys( fea );
fea       = parseprob( fea );
fea.sol.u = solvestat( fea, 'icub', 2, 'fid', opt.fid );   % Call to stationary solver.

% Postprocessing.
if( opt.iplot>0 )
  figure
  postplot( fea, 'surfexpr', fea.phys.ms.eqn.vars{2,2}, ...
            'isoexpr', fea.phys.ms.eqn.vars{2,2}, 'isolev', 25, ...
            'arrowexpr', fea.phys.ms.eqn.vars{8,2}, 'arrowcolor', 'w', 'arrowspacing', [45 30] )
  title( 'Magnetic field' )
end

% Error checking.
Az1  = intsubd( fea.phys.ms.eqn.vars{1,2}, fea, 1 );
Az2  = intsubd( fea.phys.ms.eqn.vars{1,2}, fea, 2 );
Mf1  = intsubd( fea.phys.ms.eqn.vars{2,2}, fea, 1 );
Mf2  = intsubd( fea.phys.ms.eqn.vars{2,2}, fea, 2 );
Scb1 = intbdr(  fea.phys.ms.bdr.vars{2,2}, fea, 1 );
Scb2 = intbdr(  fea.phys.ms.bdr.vars{2,2}, fea, 2 );
Scb3 = intbdr(  fea.phys.ms.bdr.vars{2,2}, fea, 3 );
Scb4 = intbdr(  fea.phys.ms.bdr.vars{2,2}, fea, 4 );
out.err = [ abs(Az1+3.178289e-8)/3.178289e-8 ;
            abs(Az2+3.178289e-8)/3.178289e-8 ;
            abs(Mf1-0.389419)/0.389419 ;
            abs(Mf2-0.520161)/0.520161 ;
            abs(Scb1-0.16233)/0.16233 ;
            abs(Scb2-0.836552)/0.836552 ;
            abs(Scb3+0.83767)/0.83767 ;
            abs(Scb4+0.16343)/0.16343 ];
out.pass = all( out.err < opt.tol );

if ( nargout==0 )
  clear fea out
end


function varargout = log(varargin)
%LOG   Natural logarithm of a CHEBFUN2.
%   LOG(F) is the natural logarithm of F. This function returns an error
%   if the function passes through or becomes numerically close to zero.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

[varargout{1:nargout}] = log@separableApprox(varargin{:});

end


function XYZ = rspd2xyz(varargin)
%RSPD2XYZ Convert relative spectral power density to XYZ.
%   RSPD2XYZ(lambda,phi) converts a relative spectral power density
%   function to 1931 CIE XYZ tristimulus values. The inputs, lambda and
%   phi, are both vectors with the same length. The input lambda is the
%   wavelength (in nm). The input phi is the relative spectral power
%   density.
%
%   RSPD2XYZ(S) converts a relative spectral power density function
%   represented as either a Px2 numeric matrix or a table. If S is a
%   numeric matrix, then S(:,1) is the wavelength, lambda, and S(:,2) is
%   phi. If S is a table with a variable called "lambda", then that
%   variable is used as lambda and the table's other variable is used as
%   phi. If the table does not have a variable named "lambda", then the
%   table's first variable is used as lambda, and the second variable is
%   used as phi.
%
%   RSPD2XYZ(I,R) converts an illumination spectral power density curve
%   (I) and a relative reflectance curve (R) to tristimulus values. I
%   and R can each be either a Px2 matrix or a table.

%   Written by Steve Eddins to accompany Digital Image Processing Using
%   MATLAB, 3rd edition, Gatesmark Press, 2020,
%   http://imageprocessingplace.com.
%
%   Copyright 2019 The MathWorks, Inc.
%   License: https://github.com/mathworks/matlab-color-tools/blob/master/license.txt

narginchk(1,2);

if nargin == 2
   if isvector(varargin{1}) && isvector(varargin{2})
      % rspd2xyz(lambda,phi)
      % Treat the input as I (the illumination curve). Construct the
      % reflectance curve as a constant from lambda = 300 to lambda =
      % 830.
      lambda = varargin{1};
      phi = varargin{2};
      I = [lambda(:) phi(:)];
      R = [300 1; 830 1];
   else
      % rspd2xyz(I,R)
      I = varargin{1};
      R = varargin{2};
   end
else
   % rspd2xyz(S)
   % Treat the input as I (the illumination curve). Construct the
   % reflectance curve as a constant from lambda = 300 to lambda = 830.
   I = varargin{1};
   R = [300 1; 830 1];
end

% Convert the inputs, in either Px2 matrix or table form, to function
% handles that return phi(lambda) for any lambda using spline
% interpolation and constant extrapolation.
fI = phiFunction(I);
fR = phiFunction(R);

matching_fcns = colorMatchingFunctions;

L = matching_fcns.lambda;
x = matching_fcns.x;
y = matching_fcns.y;
z = matching_fcns.z;
x = x/(100*sum(x));
y = y/(100*sum(y));
z = z/(100*sum(z));

% Perform a simple numerical integration.
XYZ = sum(fI(L) .* fR(L) .* [x y z],1);

%----------------------------------------------------------------------%
function f = phiFunction(S)
%   Create a function handle that returns phi(lambda). The input can be
%   a Px2 numeric matrix or a table as described in the documentation
%   above for rspd2xyz.

if istable(S)
   % Look for table variable called lambda.
   idx = find(S.Properties.VariableNames == "lambda",1);
   if isempty(idx)
      % The table does not have a lambda variable. Assume the first
      % variable is lambda and the second variable is phi.
      lambda = S{:,1};
      phi = S{:,2};
   else
      lambda = S{:,idx};

      % Use the first non-lambda table variable as phi.
      kk = 1:width(S);
      kk(idx) = [];
      phi = S{:,kk(1)};
   end
else
   % S is a matrix. Use the first column as lambda and the second
   % column as phi.
   S = double(S);
   lambda = S(:,1);
   phi = S(:,2);
end

% Add zeros to the beginning and end of phi. When using the spline
% function for cubic spline interpolation, this has the effect of
% enabling constant extrapolation using the first and last values of
% phi. See section 7.2.2.1, Extrapolation, CIE 15:2004, Technical Report
% on Colorimetry.
phi = [0 ; phi ; 0];

% Create a function handle that interpolates phi using cubic spline
% interpolation.  Cubic spline is one of the recommended interpolation
% methods. See section 7.2.1.1, Interpolation, CIE 15:2004, Technical
% Report on Colorimetry.
f = @(lambda_q) spline(lambda,phi,lambda_q);


classdef TiGE2 < ALGORITHM
% <many> <real/integer/label/binary/permutation> <constrained/none>
% Tri-Goal Evolution Framework for CMaOPs

%------------------------------- Reference --------------------------------
% Y. Zhou, Z. Min, J. Wang, Z. Zhang, and J.Zhang, Tri-goal evolution
% framework for constrained many-objective optimization, IEEE Transactions
% on Systems Man and Cybernetics Systems, 2020, 50(8): 3086-3099.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        function main(Algorithm,Problem)
            %% Parameter setting
            [Epsilon0,row] = Algorithm.ParameterSet(0.05,1.01);

            %% Generate random population
            Population = Problem.Initialization();
            [fpr,fcd]  = Estimation(Population.objs,1/Problem.N^(1/Problem.M));
            fcv        = Calculate_fcv(Population);
            Epsilon    = Epsilon0;
            PopObj_1   = [fpr,fcd];
            [fm,~]     = NDSort(PopObj_1,Problem.N);
            PopObj     = [fm' + Epsilon * fcv,fcv];
            [frank,~]  = NDSort(PopObj,Problem.N);
            fitness    = frank' + fcv./(fcv+1);

            %% Optimization
            while Algorithm.NotTerminated(Population)
                MatingPool = TournamentSelection(2,Problem.N,fitness);
                Offspring  = OperatorGA(Problem,Population(MatingPool));
                [fpr,fcd]  = Estimation(Offspring.objs,1/Problem.N^(1/Problem.M));
                fcv = Calculate_fcv(Offspring);
                OffObj_1   = [fpr,fcd];
                [fm,~] = NDSort(OffObj_1,Problem.N);
                OffObj = [fm' + Epsilon * fcv,fcv];
                [Population,fitness] = EnvironmentalSelection([Population,Offspring],PopObj,OffObj,Problem.N);
                [fpr,fcd] = Estimation(Population.objs,1/Problem.N^(1/Problem.M));
                fcv = Calculate_fcv(Population);
                PopObj_1 = [fpr,fcd];
                [fm,~]   = NDSort(PopObj_1,Problem.N);
                PopObj   = [fm' + Epsilon * fcv,fcv];
                Epsilon  = row * Epsilon;
            end
        end
    end
end


function blas1_z_test10 ( )

%*****************************************************************************80
%
%% TEST10 tests ZDROT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2006
%
%  Author:
%
%    John Burkardt
%
  n = 6;

  for j = 1 : n
    x(j) = 10 * j + j * i;
  end

  for j = 1 : n
    y(j) = 20 * j + 2 * j * i;
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST10\n' );
  fprintf ( 1, '  ZDROT carries out a Givens rotation\n' );
  fprintf ( 1, '  on a complex vector.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  X and Y\n' );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %6d  %10f  %10f  %10f  %10f\n', ...
      j, real ( x(j) ), imag ( x(j) ), real ( y(j) ), imag ( y(j) ) );
  end

  c = 0.5;
  s = sqrt ( 1.0 - c * c );
  [ x, y ] = zdrot ( n, x, 1, y, 1, c, s );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  ZDROT ( N, X, 1, Y, 1, %f, %f )\n', c, s );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %6d  %10f  %10f  %10f  %10f\n', ...
      j, real ( x(j) ), imag ( x(j) ), real ( y(j) ), imag ( y(j) ) );
  end

  return
end


function [mixparams, y, z, a] = mdnfwd(net, x)
%MDNFWD	Forward propagation through Mixture Density Network.
%
%	Description
%	 MIXPARAMS = MDNFWD(NET, X) takes a mixture density network data
%	structure NET and a matrix X of input vectors, and forward propagates
%	the inputs through the network to generate a structure MIXPARAMS
%	which contains the parameters of several mixture models.   Each row
%	of X represents one input vector and the corresponding row of the
%	matrices in MIXPARAMS  represents the parameters of a mixture model
%	for the conditional probability of target vectors given the input
%	vector.  This is not represented as an array of GMM structures to
%	improve the efficiency of MDN training.
%
%	The fields in MIXPARAMS are
%	  type = 'mdnmixes'
%	  ncentres = number of mixture components
%	  dimtarget = dimension of target space
%	  mixcoeffs = mixing coefficients
%	  centres = means of Gaussians: stored as one row per pattern
%	  covars = covariances of Gaussians
%	  nparams = number of parameters
%
%	[MIXPARAMS, Y, Z] = MDNFWD(NET, X) also generates a matrix Y of the
%	outputs of the MLP and a matrix Z of the hidden unit activations
%	where each row corresponds to one pattern.
%
%	[MIXPARAMS, Y, Z, A] = MLPFWD(NET, X) also returns a matrix A  giving
%	the summed inputs to each output unit, where each row  corresponds to
%	one pattern.
%
%	See also
%	MDN, MDN2GMM, MDNERR, MDNGRAD, MLPFWD
%

%	Copyright (c) Ian T Nabney (1996-2001)
%	David J Evans (1998)

% Check arguments for consistency
errstring = consist(net, 'mdn', x);
if ~isempty(errstring)
  error(errstring);
end

% Extract mlp and mixture model descriptors
mlpnet = net.mlp;
mixes = net.mdnmixes;

ncentres = mixes.ncentres;	% Number of components in mixture model
dim_target = mixes.dim_target;	% Dimension of targets
nparams = mixes.nparams;	% Number of parameters in mixture model

% Propagate forwards through MLP
[y, z, a] = mlpfwd(mlpnet, x);

% Compute the postion for each parameter in the whole
% matrix.  Used to define the mixparams structure
mixcoeff  = [1:1:ncentres];
centres   = [ncentres+1:1:(ncentres*(1+dim_target))];
variances = [(ncentres*(1+dim_target)+1):1:nparams];

% Convert output values into mixture model parameters

% Use softmax to calculate priors
% Prevent overflow and underflow: use same bounds as glmfwd
% Ensure that sum(exp(y), 2) does not overflow
maxcut = log(realmax) - log(ncentres);
% Ensure that exp(y) > 0
mincut = log(realmin);
temp = min(y(:,1:ncentres), maxcut);
temp = max(temp, mincut);
temp = exp(temp);
mixpriors = temp./(sum(temp, 2)*ones(1,ncentres));

% Centres are just copies of network outputs
mixcentres =  y(:,(ncentres+1):ncentres*(1+dim_target));

% Variances are exp of network outputs
mixwidths = exp(y(:,(ncentres*(1+dim_target)+1):nparams));

% Now build up all the mixture model weight vectors
ndata = size(x, 1);

% Return parameters
mixparams.type = mixes.type;
mixparams.ncentres = mixes.ncentres;
mixparams.dim_target = mixes.dim_target;
mixparams.nparams = mixes.nparams;

mixparams.mixcoeffs = mixpriors;
mixparams.centres   = mixcentres;
mixparams.covars      = mixwidths;


function [gKern1, gKern2, gVarmeans, gVarcovars, gInd] = linard2biasVardistPsi2Gradient(linardKern, biasKern, vardist, Z, covGrad, learnInducing)

% LINARD2BIASVARDISTPSI2GRADIENT description.

% VARGPLVM

if nargin < 6
    learnInducing = 1;
end

% variational means
N = size(vardist.means,1);
%  inducing variables
[M Q] = size(Z);

[Psi2, Pnobias, Psi1] = linard2biasVardistPsi2Compute(linardKern, biasKern, vardist, Z);

% inverse variances
A = linardKern.inputScales;

% gradient for the bias parameter
gKern2 = sum(sum(Pnobias.*covGrad));
Bnm = biasKern.variance*ones(size(Psi1));
BPsi1Covg = (Bnm*covGrad);

gInd = zeros(M, Q);

for q=1:vardist.latentDimension
   %
   gKern(q) = sum(sum((vardist.means(:,q)*(Z(:,q)')).*BPsi1Covg));

   gVarmeans(:,q) = A(q)*sum((ones(vardist.numData,1)*Z(:,q)').*BPsi1Covg,2);
   if learnInducing
        gInd(:,q) = A(q)*sum((vardist.means(:,q)*ones(1,size(Z,1))).*BPsi1Covg,1)';
   end
   %
end
%

gKern1 = 2*gKern(:)';
% gVarmeans is N x Q matrix (N:number of data, Q:latent dimension)
% this will unfold this matrix column-wise
gVarmeans = 2*gVarmeans(:)';

% gInd is M x Q matrix (M:number of inducing variables, Q:latent dimension)
% this will unfold this matrix column-wise
gInd = 2*gInd(:)';

gVarcovars = zeros(1,prod(size(vardist.covars)));


function test_bug2502

% WALLTIME 00:10:00
% MEM 1gb
% DEPENDENCY ft_freqstatistics ft_selectdata

% see bug 2502 for more info

% make some data
data1.someparameter = randn(100,10,15);
data1.dimord = 'subj_chan_freq';
data1.freq   = 1:15;
for k = 1:10
  data1.label{k} = ['chan',num2str(k)];
end
data2 = data1;
data2.someparameter = randn(100,10,15);

cfg = [];
cfg.method = 'montecarlo';
cfg.parameter = 'someparameter';
cfg.statistic = 'ft_statfun_indepsamplesT';
cfg.numrandomization = 0;
cfg.design = [ones(1,100) ones(1,100)*2];
stat = ft_freqstatistics(cfg, data1, data2);


function [ alphas, betas, scaling, finalLikelihood] = CCRF_training_bfgs( num_seqs, thresholdX, thresholdFun, x, y, yUnnormed, alphas, betas, lambda_a, lambda_b, similarityFNs, Precalc_Bs, Precalc_Bs_flat, Precalc_yBys, varargin)
%GRADIENTDESCENTCCRF Performs CCRF gradient descen given the initial state
%and gradient descent parameters
%   Detailed explanation goes here

    % if these are not provided calculate them, TODO this might be

    % It is possible to predefine the component B^(k) required
    % to compute B term and partial derivatives, also can predefine yB^(k)y,
    % as they also do not change through the iterations
    if(sum(strcmp(varargin,'PrecalcBs')) && sum(strcmp(varargin,'PrecalcBsFlat'))...
             && sum(strcmp(varargin,'Precalc_yBy')))

        ind = find(strcmp(varargin,'PrecalcBs')) + 1;
        Precalc_Bs = varargin{ind};

        ind = find(strcmp(varargin,'PrecalcBsFlat')) + 1;
        Precalc_Bs_flat = varargin{ind};

        ind = find(strcmp(varargin,'Precalc_yBys')) + 1;
        Precalc_yBys = varargin{ind};
    else
        % if these are not provided calculate them
        [ ~, Precalc_Bs, Precalc_Bs_flat, Precalc_yBys ] = CalculateSimilarities( num_seqs, x, similarityFNs, y);
    end

    params = [alphas; betas];

    objectiveFun = @(params)objectiveFunction(params, numel(alphas), lambda_a, lambda_b, Precalc_Bs, x, y, Precalc_yBys, Precalc_Bs_flat);

    options = optimset('Algorithm','interior-point','GradObj','on', 'TolX', thresholdX, 'TolFun', thresholdFun, 'Hessian', 'bfgs', 'display','off', 'useParallel', 'Always');

    if(sum(strcmp(varargin,'max_iter')))
        options.MaxIter = varargin{find(strcmp(varargin,'max_iter')) + 1};
    end

    params = fmincon(objectiveFun, params, [], [],[],[], zeros(numel(params),1), Inf(numel(params), 1), [], options);
    alphas = params(1:numel(alphas));
    betas = params(numel(alphas)+1:end);

    finalLikelihood = LogLikelihoodCCRF(y, x, alphas, betas, lambda_a, lambda_b, Precalc_Bs_flat);
%     fprintf('Final log likelihood at iteration; logL %f, learning rate\n', finalLikelihood);

    % establish the scaling
    scaling = getScaling2(alphas, betas, x, yUnnormed, Precalc_Bs);

end

function [loss, gradient] = objectiveFunction(params, numAlpha, lambda_a, lambda_b, PrecalcBs, x, y, Precalc_yBys, PrecalcBsFlat)

    alphas = params(1:numAlpha);
    betas = params(numAlpha+1:end);
    [gradient, SigmaInvs, CholDecomps, Sigmas] = gradientCCRFFull(params, lambda_a, lambda_b, PrecalcBs, x, y, Precalc_yBys, PrecalcBsFlat);
    % as bfgs does gradient descent rather than ascent, negate the results
    gradient = -gradient;
    loss = -LogLikelihoodCCRF(y, x, alphas, betas, lambda_a, lambda_b, PrecalcBsFlat, SigmaInvs, CholDecomps, Sigmas);
end


function gradu = singletrack_gradu_f(in1,in2,in3)
%SINGLETRACK_GRADU_F
%    GRADU = SINGLETRACK_GRADU_F(IN1,IN2,IN3)

%    This function was generated by the Symbolic Math Toolbox version 8.3.
%    09-Jan-2020 11:52:34

I_z = in3(2,:);
M = in3(1,:);
T = in2(2,:);
V_vx = in1(4,:);
V_vy = in1(5,:);
c_f = in3(8,:);
delta = in2(1,:);
deltamax = in3(5,:);
l_f = in3(3,:);
maxbrakeWForce = in3(6,:);
maxmotorWForce = in3(7,:);
psi_dot = in1(6,:);
t2 = 1.0./I_z;
t3 = 1.0./M;
t4 = T.*5.0e+1;
t5 = delta.*5.0e+1;
t6 = deltamax.*5.0e+1;
t8 = V_vy.*1i;
t9 = V_vx.*1.0e+2;
t13 = l_f.*psi_dot.*1i;
t7 = -t4;
t10 = -t5;
t11 = -t6;
t12 = tanh(t9);
t24 = V_vx+t8+t13;
t14 = t7+5.0e+1;
t15 = t7-5.0e+1;
t18 = t6+t10;
t21 = t10+t11;
t26 = angle(t24);
t16 = exp(t14);
t17 = exp(t15);
t19 = exp(t18);
t22 = exp(t21);
t20 = t16+1.0;
t23 = t17+1.0;
t25 = t19+1.0;
t29 = t22+1.0;
t27 = 1.0./t20;
t30 = 1.0./t23;
t32 = 1.0./t25;
t34 = 1.0./t29;
t28 = t27.^2;
t31 = t30.^2;
t33 = t32.^2;
t35 = t34.^2;
t36 = t27.*5.0e+1;
t37 = t27-1.0;
t38 = t30.*5.0e+1;
t40 = deltamax.*t32;
t41 = t32-1.0;
t43 = t34-1.0;
t39 = -t36;
t42 = -t38;
t44 = -t40;
t45 = deltamax.*t43;
t47 = t16.*t28.*5.0e+1;
t49 = t17.*t31.*5.0e+1;
t50 = t16.*t28.*2.5e+3;
t53 = t17.*t31.*2.5e+3;
t55 = t6.*t19.*t33;
t56 = deltamax.*t19.*t33.*-5.0e+1;
t57 = t6.*t22.*t35;
t58 = t30.*t37;
t59 = deltamax.*t22.*t35.*-5.0e+1;
t61 = t37.*t38;
t64 = t34.*t41;
t67 = t4.*t16.*t28.*t30;
t69 = t4.*t17.*t31.*t37;
t70 = t5.*t19.*t33.*t34;
t72 = t5.*t22.*t35.*t41;
t46 = -t45;
t48 = -t47;
t51 = -t49;
t52 = -t50;
t54 = -t53;
t60 = T.*t58;
t63 = t4.*t58;
t65 = delta.*t64;
t68 = T.*t30.*t50;
t71 = T.*t37.*t53;
t90 = t56+t59+t64+t70+t72;
t62 = -t60;
t66 = -t65;
t74 = t39+t42+t63+5.0e+1;
t86 = t26+t44+t46+t65;
t87 = t48+t51+t58+t67+t69;
t89 = t52+t54+t61+t68+t71;
t73 = t30+t37+t62;
t75 = exp(t74);
t77 = t40+t45+t66;
t76 = t75+1.0;
t78 = sin(t77);
t81 = cos(t77);
t79 = 1.0./t76;
t80 = t79.^2;
t82 = maxmotorWForce.*t79;
t83 = t79-1.0;
t84 = maxbrakeWForce.*t12.*t83;
t91 = maxmotorWForce.*t75.*t80.*t89;
t92 = maxbrakeWForce.*t12.*t75.*t80.*t89;
t85 = -t84;
t93 = -t92;
t88 = t82+t85;
t94 = t91+t93;
gradu = reshape([0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,t3.*(c_f.*t78.*t90-c_f.*t81.*t86.*t90+(t73.*t78.*t88.*t90)./2.0),-t3.*((t73.*t94)./2.0+(t87.*t88)./2.0+(t73.*t81.*t94)./2.0+(t81.*t87.*t88)./2.0),0.0,-t3.*(c_f.*t81.*t90+c_f.*t78.*t86.*t90+(t73.*t81.*t88.*t90)./2.0),-t3.*((t73.*t78.*t94)./2.0+(t78.*t87.*t88)./2.0),0.0,-t2.*(c_f.*l_f.*t81.*t90+c_f.*l_f.*t78.*t86.*t90+(l_f.*t73.*t81.*t88.*t90)./2.0),-t2.*((l_f.*t73.*t78.*t94)./2.0+(l_f.*t78.*t87.*t88)./2.0),0.0,0.0,0.0,1.0],[3,7]);


function [varargout]=tri2quad(varargin)

% function [Fq,Vq,Cq]=tri2quad(Ft,Vt,convertMethod)

%%
switch nargin
    case 2
        Ft=varargin{1};
        Vt=varargin{2};
        convertMethod=1;
    case 3
        Ft=varargin{1};
        Vt=varargin{2};
        convertMethod=varargin{3};
end

%Cope with 2D input
if size(Vt,2)==2
    Vt(:,3)=0;
end
%% Mid edge sets

edgeMat=[Ft(:,[1 2]); Ft(:,[2 3]);  Ft(:,[3 1])];

E_sort=sort(edgeMat,2); %Sorted edges matrix
[~,ind1,~]=unique(E_sort,'rows');
edgeMat=edgeMat(ind1,:);

numPoints = size(Vt,1);
numEdges = size(edgeMat,1);

% Get indices of the four edges associated with each face
A = sparse(edgeMat(:,1),edgeMat(:,2),(1:numEdges)+numPoints,numPoints,numPoints,numEdges);
A = max(A,A'); %Copy symmetric

%Indices for A matrix
indA_12=Ft(:,1)+(Ft(:,2)-1)*numPoints;
indA_23=Ft(:,2)+(Ft(:,3)-1)*numPoints;
indA_31=Ft(:,3)+(Ft(:,1)-1)*numPoints;

%Get indices for vertex array
indV_12=full(A(indA_12));
indV_23=full(A(indA_23));
indV_31=full(A(indA_31));

%% Mid face

% Ft_sort=sort(Ft,2); %Sorted edges matrix
% [~,ind1,ind2]=unique(Ft_sort,'rows');
% Ft=Ft(ind1,:);

indV_midFace=(1:1:size(Ft,1))';
indOffset=numPoints+size(edgeMat,1);

indV_midFace123=(indV_midFace((1-1)*size(Ft,1)+(1:size(Ft,1))))+indOffset;

%% Create quad faces array

 Fq=[Ft(:,1) indV_12 indV_midFace123 indV_31;... %Corner quad 1
    indV_12  Ft(:,2) indV_23 indV_midFace123;... %Corner quad 2
     indV_midFace123 indV_23 Ft(:,3) indV_31;... %Corner quad 3
    ];

%% Create vertex arrays

%new mid-edge points
Vn=0.5*(Vt(edgeMat(:,1),:)+Vt(edgeMat(:,2),:));

switch convertMethod
    case 1
        %new mid-element points
        Vm=zeros(size(Ft,1),3);
        for q=1:1:size(Vt,2)
            X=Vt(:,q);
            if size(Ft,1)==1
                Vm(:,q)=mean(X(Ft)',2);
            else
                Vm(:,q)=mean(X(Ft),2);
            end
        end
    case 2
        %new mid-face points
        TR=triangulation(Ft,Vt);
        Vm = incenter(TR,(1:size(Ft,1))');
end

Vq=[Vt; Vn; Vm]; %Join point sets

CVq=[0*ones(size(Vt,1),1); 1*ones(size(Vn,1),1); 2*ones(size(Vm,1),1);];

%%

varargout{1}=Fq;
varargout{2}=Vq;
varargout{3}=CVq;

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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 3 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, see <http://www.gnu.org/licenses/>.


function Datacube = Pooling(ktensor)
% input
%   ktensor(Cell Array): Each cell contains a ktensor caculated by a given
%   channel of convolutionized Datacube
% output
%   Datacube(Cell Array): For each cell of ktensor, discarding the third
%   dimension, outproduct the first and the second dimension with
%   corresponding lambda to form a Datacube with equal size of space
%   Maxpooling ?
num_channels = size(ktensor,1);
Datacube = cell(num_channels,1);
for i=1:num_channels
    num_of_U = size(ktensor{i}.U{1},2);
    combined_basis = [];
    for j=1:num_of_U
        temp = ktensor{i}.lambda(j) * ktensor{i}.U{1}(:,j) * ktensor{i}.U{2}(:,j)';
        combined_basis(:,:,j) = temp;
    end
    Datacube{i} = combined_basis;
end


function L1 = calc_Laplacian_for_H1(B1)
B = B1 - 1;
L = calc_Laplacian_for_H(B);
L1 = zeros(B1,B1);
L1(2:end,2:end) = L;

function L = calc_Laplacian_for_H(B)
W = diag(ones(1,B-1),-1) + diag(ones(1,B-1),1);
D = diag(sum(W,2));
L = D - W;


function [ddydphi, d2gdx2, logL, dy, dy2, vy]= VBA_get_dL (gx, dG_dPhi, y, type, Qy, sigmaHat)
% // VBA toolbox //////////////////////////////////////////////////////////
%
% [ddydphi, d2gdx2, logL, dy, dy2, vy]= VBA_get_dL (gx, dG_dPhi, y, type, Qy, sigmaHat)
% Compute usefull intermediate values describing the misfit between a model
% prediction and an observation.
%
% IN:
%   - gx: model prediction about the observation y (1st order moment)
%   - dG_dPhi: derivative of gx wrt observation parameters
%   - y: actual observation
%   - type: flag defining the distribution of the observation (0: gaussian,
%     1: Bernouilli, 2: categorical)
%   - Qy: scaling matrix of the 2nd order moment of the prediction (only
%     gaussian observations)
%   - sigmaHat: scaling factor of the 2nd order moment of the prediction
%     (only gaussian observations)
%
% OUT:
%   - ddydphi: gradient of the prediction error wrt observation parameters
%   - d2gdx2: hessian of the prediction
%   - logL: log-likelihood of the observation given the prediction
%   - dy: prediction error
%   - dy2: normalized squared deviation
%   - vy: prediction variance
%
% /////////////////////////////////////////////////////////////////////////

if nargin<5
end

% prediction error
dy = y - gx;

switch type

    case 0 %--- normal
        vy=(1./sigmaHat).*diag(VBA_inv(Qy));
        ddydphi = sigmaHat.*(dG_dPhi*Qy*dy);
        d2gdx2 = sigmaHat.*(dG_dPhi*Qy*dG_dPhi');
        dy2=dy'*Qy*dy ;
        logL = - 0.5*sigmaHat.*dy2 ;
        logL = logL + 0.5*VBA_logDet(Qy*sigmaHat) - 0.5*numel(dy2)*log(2*pi) ;

    case 1 %--- binomial
        gx = VBA_finiteBinomial (gx);
        vy = gx.*(1-gx) ;
        ddydphi = dG_dPhi*(dy./vy);
        temp = y./(gx).^2 - (y-1)./(1-gx).^2;
        d2gdx2 = dG_dPhi*diag(temp)*dG_dPhi';
        logL = y'*log(gx) + (1-y)'*log(1-gx);
        dy2 = sum(temp);

   case 2   %--- multinomial
        gx = VBA_finiteBinomial (gx);
        vy = gx.*(1-gx) ;
        ddydphi = dG_dPhi*(y./gx);
        d2gdx2 = dG_dPhi*diag(y./gx.^2)*dG_dPhi';
        dy2 = sum(y./(gx).^2);
        logL = log(gx)'*y;

end

end


function [vertices,faces] = mesh_freesurferTRI2matlab(file)

% mesh_freesurferTRI2matlab - Read FreeSurfer tesselation (.tri)
%
% USEAGE: [vertices,faces] = mesh_freesurferTRI2matlab(file)
%
% This function will load an ascii file that contains a one
% line specification of the number of vertices followed
% by rows of vertex points.  It then reads a one line
% specification of the number of faces followed by rows
% of face indices into the vertex rows.  Each vertex row
% contains a vertex index number and 3 x,y,z coordinates.
% Each face row contains a face index and three vertex
% indices.  Vertices in the .tri file are indexed from one
% and those returned are indexed from one.
%
% See also the mesh_freesurfer2matlab function to load
% the tesselations that are created by the mris_convert
% function of freesurfer, which have a different text format
% from those of the BEM .tri files.
%
% The freesurfer tesselations may contain too many faces
% for efficient computations.  If so, try 'reducepatch'.
%
% The returned matrices can be input to the patch command, like so:
%
%    Hpatch = patch('Vertices',vertices,'Faces',faces,...
%                   'EdgeColor',[.8 .8 .8],'FaceColor',[0.9 0.9 0.9]);
%
% This will plot the mesh as a patch object.  See the patch command
% and matlab help for more information on coloring this object.
%

% $Revision: 1.1 $ $Date: 2009-04-28 22:13:57 $

% Licence:  GNU GPL, no implied or express warranties
% History:  03/02 Darren.Weber_at_radiology.ucsf.edu
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

fid = fopen(file,'r');

if isequal(fid,-1),
    S=sprintf('Could not open file: "%s"',file);
    error(S);
else

    fprintf('...Reading FreeSurfer Tesselation (.tri)\n');

    tic;

    % Check for comment on first line of file
    frewind(fid); temp = fscanf(fid,'%s',1); frewind(fid);
    if findstr(temp,'#'), temp = fgetl(fid); end

    % Read vertices
    Nvertices = fscanf(fid,'%d',1);
    fprintf('...Reading %d Vertices\n',Nvertices);
    vertices = fscanf(fid,'%f',[4,Nvertices]);
    % remove first row (index) and translate
    vertices = vertices(2:4,:)';

    % Read faces
    Nfaces    = fscanf(fid,'%d',1);
    fprintf('...Reading %d Faces\n',Nfaces);
    faces = fscanf(fid,'%d',[4,Nfaces]);
    % remove first row (index) & translate
    faces = faces(2:4,:)';

    fclose(fid);

    t = toc;
    fprintf('...done (%6.2f sec).\n',t);

end

return


function g = diffy(f, n)
%DIFFY   Differentiate a CHEBFUN2 with respect to its second argument.
%
%   G = DIFFY(F) returns a CHEBFUN2 representing the derivative of F in its
%   second argument. This is the same as DIFF(F,1,1).
%
%   G = DIFFY(F,N) returns a CHEBFUN2 representing the Nth derivative of F in
%   its second argument. This is the same as DIFF(F,N,1).
%
%   This command is for convenience as the syntax for DIFF, inherited from the
%   DIFF command for matrices, can be confusing.
%
% See also DIFFX, DIFF.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Default to first derivative:
if ( nargin == 1 )
    n = 1;
end

% Call diff:
g = diff(f, n, 1);

end


function x = emailFeatures(word_indices)
%EMAILFEATURES takes in a word_indices vector and produces a feature vector
%from the word indices
%   x = EMAILFEATURES(word_indices) takes in a word_indices vector and
%   produces a feature vector from the word indices.

% Total number of words in the dictionary
n = 1899;

% You need to return the following variables correctly.
x = zeros(n, 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Fill in this function to return a feature vector for the
%               given email (word_indices). To help make it easier to
%               process the emails, we have have already pre-processed each
%               email and converted each word in the email into an index in
%               a fixed dictionary (of 1899 words). The variable
%               word_indices contains the list of indices of the words
%               which occur in one email.
%
%               Concretely, if an email has the text:
%
%                  The quick brown fox jumped over the lazy dog.
%
%               Then, the word_indices vector for this text might look
%               like:
%
%                   60  100   33   44   10     53  60  58   5
%
%               where, we have mapped each word onto a number, for example:
%
%                   the   -- 60
%                   quick -- 100
%                   ...
%
%              (note: the above numbers are just an example and are not the
%               actual mappings).
%
%              Your task is take one such word_indices vector and construct
%              a binary feature vector that indicates whether a particular
%              word occurs in the email. That is, x(i) = 1 when word i
%              is present in the email. Concretely, if the word 'the' (say,
%              index 60) appears in the email, then x(60) = 1. The feature
%              vector should look like:
%
%              x = [ 0 0 0 0 1 0 0 0 ... 0 0 0 0 1 ... 0 0 0 1 0 ..];
%
%

for i = 1:size(word_indices,1),
  x(word_indices(i)) = 1;
end

% =========================================================================

end


function areasmooth = SmoothAreaOutliers(area,filterorder,maxfreq,maxerr)

areasmooth = LowPassFilterArea(area,filterorder,maxfreq);
isoutlier = isinf(area) | isnan(area) | abs(areasmooth - area) > maxerr;
[starts,ends] = get_interval_ends(isoutlier);
ends = ends - 1;
areasmooth = area;
nframes = numel(area);
for i = 1:numel(starts),
  if starts(i) == 1 && ends(i) == nframes,
    break;
  elseif starts(i) == 1,
    areasmooth(starts(i):ends(i)) = area(ends(i)+1);
  elseif ends(i) == nframes,
    areasmooth(starts(i):ends(i)) = area(starts(i)-1);
  else
    areasmooth(starts(i):ends(i)) = (area(starts(i)-1)+area(ends(i)+1))/2;
  end
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   PARAMETERS Returns a data structure containing the parameters of the
%   ABB IRB140.
%
%   Author: Arturo Gil. Universidad Miguel Hernandez de Elche.
%   email: arturo.gil@umh.es date:   09/01/2012
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% ARTE 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
function robot = parameters()

robot.name= 'KUKA_KR20_3';

robot.DH.theta= '[q(1) q(2)-pi/2 q(3) q(4) q(5) q(6)+pi]';
robot.DH.d='[0.675 0 0 0.67 0 0.158]';
robot.DH.a='[0.26 0.68 -0.035 0 0 0]';
robot.DH.alpha= '[-pi/2 0 -pi/2 pi/2 -pi/2 0]';
robot.J=[];

robot.inversekinematic_fn = 'inversekinematic_kuka_kr20_3(robot, T)';
robot.directkinematic_fn = 'directkinematic(robot, q)';

%number of degrees of freedom
robot.DOF = 6;

%rotational: 0, translational: 1
robot.kind=['R' 'R' 'R' 'R' 'R' 'R'];

%minimum and maximum rotation angle in rad
robot.maxangle =[deg2rad(-185) deg2rad(185); %Axis 1, minimum, maximum
                deg2rad(-155) deg2rad(35); %Axis 2, minimum, maximum
                deg2rad(-130) deg2rad(154); %Axis 3
                deg2rad(-350) deg2rad(350); %Axis 4: Unlimited (400� default)
                deg2rad(-130) deg2rad(130); %Axis 5
                deg2rad(-350) deg2rad(350)]; %Axis 6: Really Unlimited to (800� default)

%maximum absolute speed of each joint rad/s or m/s
robot.velmax = [deg2rad(156); %Axis 1, rad/s
                deg2rad(156); %Axis 2, rad/s
                deg2rad(156); %Axis 3, rad/s
                deg2rad(330); %Axis 4, rad/s
                deg2rad(332); %Axis 5, rad/s
                deg2rad(616)];%Axis 6, rad/s

robot.accelmax=robot.velmax/0.1; % 0.1 is here an acceleration time

% end effectors maximum velocity
robot.linear_velmax = 2.5; %m/s

%base reference system
robot.T0 = eye(4);

%INITIALIZATION OF VARIABLES REQUIRED FOR THE SIMULATION
%position, velocity and acceleration
robot=init_sim_variables(robot);
robot.path = pwd;

% GRAPHICS
robot.graphical.has_graphics=1;
robot.graphical.color = [255 102 51]./255;
%for transparency
robot.graphical.draw_transparent=0;
%draw DH systems
robot.graphical.draw_axes=0;
%DH system length and Font size, standard is 1/10. Select 2/20, 3/30 for
%bigger robots
robot.graphical.axes_scale=1;
%adjust for a default view of the robot

robot.axis=[-1.1 1.7 -1.75 1.75 -0.5 2.2];
%read graphics files
robot = read_graphics(robot);

%DYNAMICS
robot.has_dynamics=0;


function [shifts,Mf] = register_frame(Yt,fftTempMat,fftTempPatches,patches,options)

minY = min(Yt(:));
maxY = max(Yt(:));
dim = size(Yt);
Yc = split_frame(Yt,patches);
fftY = fft(fft(fft(Yc,[],1),[],2),[],3);
nd = ndims(Yt);
n_patches = [length(unique(patches(:,1))),length(unique(patches(:,3))),length(unique(patches(:,5)))];
shifts = zeros([n_patches,nd]);
N_patches = size(patches,1);

if N_patches > 1
    if nd == 2; out_rig = dftregistration_min_max(fftTempMat,fftn(Yt),options.us_fac,-options.max_shift,options.max_shift,options.phase_flag); lb = out_rig(3:4); ub = out_rig(3:4); end
    if nd == 3; out_rig = dftregistration_min_max_3d(fftTempMat,fftn(Yt),options.us_fac,-options.max_shift,options.max_shift,options.phase_flag); lb = out_rig(3:5); ub = out_rig(3:5); end
    max_dev = options.max_dev;
else
    lb = -options.max_shift(1,nd);
    ub = options.max_shift(1,nd);
    max_dev = 0*options.max_dev;
end

for i = 1:N_patches
    [ix,iy,iz] = ind2sub(n_patches,i);
    if nd == 2
        output = dftregistration_min_max(fftTempPatches(:,:,:,i),fftY(:,:,:,i),options.us_fac,lb-max_dev(1:2),ub+max_dev(1:2),options.phase_flag);
    elseif nd == 3
        output = dftregistration_min_max_3d(fftTempPatches(:,:,:,i),fftY(:,:,:,i),options.us_fac,lb-max_dev,ub+max_dev,options.phase_flag);
        shifts(ix,iy,iz,3) = output(5);
    end

    shifts(ix,iy,iz,1) = output(3);
    shifts(ix,iy,iz,2) = output(4);
end

if nd == 3
    shifts_up = zeros([dim,3]);
    do = size(shifts_up)./size(shifts);
    ds = size(shifts);
    if (0)
        [Xq,Yq,Zq] = meshgrid(linspace((2+0*1/do(2))/2,ds(2)+0*(0-1/do(2))/2,dim(2)),linspace((2+0*1/do(1))/2,ds(1)+0*(0-1/do(1))/2,dim(1)),linspace((2+0*1/do(3))/2,ds(3)+0*(0-1/do(3))/2,dim(3)));
    else
        [Xq,Yq,Zq] = meshgrid(linspace((1+1/do(2))/2,ds(2)+(1-1/do(2))/2,dim(2)),linspace((1+1/do(1))/2,ds(1)+(1-1/do(1))/2,dim(1)),linspace((1+1/do(3))/2,ds(3)+(1-1/do(3))/2,dim(3)));
        Xq(Xq<1)=1; Xq(Xq>dim(2))=dim(2);
        Yq(Yq<1)=1; Yq(Yq>dim(1))=dim(1);
        Zq(Zq<1)=1; Zq(Zq>dim(3))=dim(3);
    end
    if numel(shifts) > 3
%       tform = affine3d(diag([options.mot_uf(:);1]));
%       for dm = 1:3; shifts_up(:,:,:,dm) = imwarp(shifts(:,:,:,dm),tform,'OutputView',imref3d([options.d1,options.d2,options.d3])); end
        for dm = 1:3; shifts_up(:,:,:,dm) = interp3(shifts(:,:,:,dm),Xq,Yq,Zq,'makima'); end
    else
        for dm = 1:3; shifts_up(:,:,:,dm) = shifts(dm); end
    end
    shifts_up(2:2:end,:,:,2) = shifts_up(2:2:end,:,:,2) + options.col_shift;
    Mf = imwarp(Yt,-cat(4,shifts_up(:,:,:,2),shifts_up(:,:,:,1),shifts_up(:,:,:,3)),'bicubic','FillValues',options.add_value);
else
    shifts_up = imresize(shifts,[options.d1,options.d2]);
    shifts_up(2:2:end,:,2) = shifts_up(2:2:end,:,2) + options.col_shift;
    Mf = imwarp(Yt,-cat(3,shifts_up(:,:,2),shifts_up(:,:,1)),'bicubic','FillValues',options.add_value);
end
Mf(Mf<minY)=minY;
Mf(Mf>maxY)=maxY;

end


function [rgb,points3d,points3dMatrix]=read_3d_pts_general(depthInpaint,K,depthInpaintsize,imageName,crop)
    %K is [fx 0 cx; 0 fy cy; 0 0 1];
    %K = frames.K;
    if ~isempty(K)
        cx = K(1,3); cy = K(2,3);
        fx = K(1,1); fy = K(2,2);
    else
        fx = 5.1885790117450188e+02;
        fy = 5.1946961112127485e+02;
        cx = 3.2558244941119034e+02;
        cy = 2.5373616633400465e+02;
    end
    invalid = depthInpaint==0;
    if ~isempty(imageName)
        im = imread(imageName);
        rgb = im2double(im);
    else
        rgb =double(cat(3,zeros(depthInpaintsize(1),depthInpaintsize(2)),...
                    ones(depthInpaintsize(1),depthInpaintsize(2)),...
                    zeros(depthInpaintsize(1),depthInpaintsize(2))));
    end
    rgb = reshape(rgb, [], 3);
    %3D points
    [x,y] = meshgrid(crop(2)-1+(1:depthInpaintsize(2)), crop(1)-1+(1:depthInpaintsize(1)));
    x3 = (x-cx).*depthInpaint*1/fx;
    y3 = (y-cy).*depthInpaint*1/fy;
    z3 = depthInpaint;
    points3dMatrix =cat(3,x3,z3,-y3);
    points3dMatrix(cat(3,invalid,invalid,invalid))=NaN;
    points3d = [x3(:) z3(:) -y3(:)];
    points3d(invalid(:),:) =NaN;
end


function dy = sparseGalerkin(t,y,ahat,polyorder,usesine)
% Copyright 2015, All Rights Reserved
% Code by Steven L. Brunton
% For Paper, "Discovering Governing Equations from Data:
%        Sparse Identification of Nonlinear Dynamical Systems"
% by S. L. Brunton, J. L. Proctor, and J. N. Kutz

yPool = poolData(y',length(y),polyorder,usesine);
dy = (yPool*ahat)';


function [m,other] = esvm_update_dfun(m)
%% Perform Distance Function learning for a single exemplar model.  We
% assume that the exemplar has a set of detections loaded in
% m.model.svxs and m.model.svbbs.
% Returns: model [m] with updated classifier
% If no arguments are given returns the suffix '-dfun' and
% classifier type 'dfun'
%
% Copyright (C) 2011-12 by Tomasz Malisiewicz
% All rights reserved.
%
% This file is part of the Exemplar-SVM library and is made
% available under the terms of the MIT license (see COPYING file).
% Project homepage: https://github.com/quantombone/exemplarsvm

other = 'dfun';
%if no inputs are specified, just return the suffix of current method
if nargin==0
  m = '-dfun';
  return;
end

%If no mask is specified, then we assume that every single bin
%should be used.
if ~isfield(m.model,'mask') | length(m.model.mask)==0
  m.model.mask = logical(ones(numel(m.model.w),1));
end

%If the mask is only MxNx1, where the feature are MxNxF, then we
%repmat the mask to match the features.  This is important because
%it is sometimes smarter to save a 2D mask is 1/F the size of the
%full mask.
if length(m.model.mask(:)) ~= numel(m.model.w)
  m.model.mask = repmat(m.model.mask,[1 1 features]);
  m.model.mask = logical(m.model.mask(:));
end

mining_params = m.mining_params;
xs = m.model.svxs;
bbs = m.model.svbbs;

% A trick in case we have too many detections
MAXSIZE = 2000;
if size(xs,2) >= MAXSIZE
  HALFSIZE = MAXSIZE/2;
  %NOTE: random is better than top 5000
  r = m.model.w(:)'*xs;
  [tmp,r] = sort(r,'descend');
  r1 = r(1:HALFSIZE);

  r = HALFSIZE+randperm(length(r((HALFSIZE+1):end)));
  r = r(1:HALFSIZE);
  r = [r1 r];
  xs = xs(:,r);
  bbs = bbs(r,:);
end

newx = cat(2,m.model.x,xs);
newx = bsxfun(@minus,newx,m.model.x(:,1)).^2;

newy = cat(1,ones(size(m.model.x,2),1),-1*ones(size(xs,2),1));

number_positives = sum(newy==1);
number_negatives = sum(newy==-1);

wpos = mining_params.train_positives_constant;
wneg = 1;

% if mining_params.BALANCE_POSITIVES == 1
%   fprintf(1,'balancing positives\n');
%   wpos = 1/number_positives;
%   wneg = 1/number_negatives;
%   wpos = wpos / wneg;
%   wneg = wneg / wneg;
% end

newx = newx(logical(m.model.mask),:);

fprintf(1,' -----\nStarting SVM dim=%d... s+=%d, s-=%d ',...
        size(newx,1), number_positives, number_negatives);
starttime = tic;

svm_model = libsvmtrain(newy, newx',sprintf(['-s 0 -t 0 -c' ...
                    ' %f -w1 %.9f -q'], mining_params.train_svm_c, wpos));

if length(svm_model.sv_coef) == 0
  %learning had no negatives
  wex = m.model.w;
  b = m.model.b;
  fprintf(1,'reverting to old model...\n');

else

  %convert support vectors to decision boundary
  svm_weights = full(sum(svm_model.SVs .* ...
                         repmat(svm_model.sv_coef,1, ...
                                size(svm_model.SVs,2)),1));

  wex = svm_weights';
  b = svm_model.rho;

  %With libsvm, the first datapoint is interpreted as class +1, so if
  %we started with a negative data point, we have to flip everything.
  if newy(1) == -1
    wex = wex*-1;
    b = b*-1;
  end

  wex2 = zeros(size(newx,1),1);
  wex2(m.model.mask) = wex;

  wex = wex2;

  %% issue a warning if the norm is very small
  if norm(wex) < .00001
    fprintf(1,'learning broke down!\n');
  end
end

maxpos = max(wex'*newx - b);
fprintf(1,' --- Max positive is %.3f\n',maxpos);

fprintf(1,'took %.3f sec\n',toc(starttime));

m.model.w = reshape(wex, size(m.model.w));
m.model.b = b;

%Take top train_max_negatives detections
r = m.model.w(:)'*bsxfun(@minus,m.model.svxs,m.model.x(:,1)).^2 - m.model.b;

[alpha,beta] = sort(r,'descend');
svs = beta(1:min(length(beta),mining_params.train_max_negatives_in_cache));
m.model.svxs = m.model.svxs(:,svs);
m.model.svbbs = m.model.svbbs(svs,:);

s = length(svs);
fprintf(1,' ---length of kept svs in dfun is: %d\n',s);


function [ radiance ] = dehaze_fast( image, omega, win_size )
%DEHZE Summary of this function goes here
%   Detailed explanation goes here

if ~exist('omega', 'var')
    omega = 0.95;
end

if ~exist('win_size', 'var')
    win_size = 15;
end

r = 15;
res = 0.001;

[m, n, ~] = size(image);

dark_channel = get_dark_channel(image, win_size);

atmosphere = get_atmosphere(image, dark_channel);

trans_est = get_transmission_estimate(image, atmosphere, omega, win_size);

x = guided_filter(rgb2gray(image), trans_est, r, res);

transmission = reshape(x, m, n);

radiance = get_radiance(image, transmission, atmosphere);

end


classdef ConvTranspose < dagnn.Layer
  properties
    size = [0 0 0 0]
    hasBias = true
    upsample = [1 1]
    crop = [0 0 0 0]
    numGroups = 1
    opts = {'cuDNN'}
  end

  methods
    function outputs = forward(obj, inputs, params)
      if ~obj.hasBias, params{2} = [] ; end
      outputs{1} = vl_nnconvt(...
        inputs{1}, params{1}, params{2}, ...
        'upsample', obj.upsample, ...
        'crop', obj.crop, ...
        'numGroups', obj.numGroups, ...
        obj.opts{:}) ;
    end

    function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
      if ~obj.hasBias, params{2} = [] ; end
      [derInputs{1}, derParams{1}, derParams{2}] = vl_nnconvt(...
        inputs{1}, params{1}, params{2}, derOutputs{1}, ...
        'upsample', obj.upsample, ...
        'crop', obj.crop, ...
        'numGroups', obj.numGroups, ...
        obj.opts{:}) ;
    end

    function outputSizes = getOutputSizes(obj, inputSizes)
      outputSizes{1} = [...
        obj.upsample(1) * (inputSizes{1}(1) - 1) + obj.size(1) - obj.crop(1) - obj.crop(2), ...
        obj.upsample(2) * (inputSizes{1}(2) - 1) + obj.size(2) - obj.crop(3) - obj.crop(4), ...
        obj.size(3), ...
        inputSizes{1}(4)] ;
    end

    function rfs = getReceptiveFields(obj)
      rfs.size = (obj.size(1:2) - 1) ./ obj.upsample + 1 ;
      rfs.stride = 1 ./ [obj.upsample] ;
      rfs.offset = (2*obj.crop([1 3]) - obj.size(1:2) + 1) ...
        ./ (2*obj.upsample) + 1 ;
    end

    function params = initParams(obj)
      % todo: test this initialization method
      sc = sqrt(2 / prod(obj.size([1 2 4]))) ;
      params{1} = randn(obj.size,'single') * sc ;
      if obj.hasBias
        params{2} = zeros(obj.size(3),1,'single') * sc ;
      end
    end

    function set.size(obj, ksize)
      % make sure that ksize has 4 dimensions
      ksize = [ksize(:)' 1 1 1 1] ;
      obj.size = ksize(1:4) ;
    end

    function set.crop(obj, crop)
      if numel(crop) == 1
        obj.crop = [crop crop crop crop] ;
      elseif numel(crop) == 2
        obj.crop = crop([1 1 2 2]) ;
      else
        obj.crop = crop ;
      end
    end

    function set.upsample(obj, upsample)
      if numel(upsample) == 1
        obj.upsample = [upsample upsample] ;
      else
        obj.upsample = upsample ;
      end
    end

    function obj = ConvTranspose(varargin)
      obj.load(varargin) ;
      % normalize field by implicitly calling setters defined in
      % dagnn.Filter and here
      obj.size = obj.size ;
      obj.upsample = obj.upsample ;
      obj.crop = obj.crop ;
    end
  end
end


function G_vctr_sym = G_vctr_fcn(in1,in2)
%G_VCTR_FCN
%    G_VCTR_SYM = G_VCTR_FCN(IN1,IN2)

%    This function was generated by the Symbolic Math Toolbox version 8.2.
%    19-Oct-2021 16:04:40

pi171 = in2(17,:);
pi191 = in2(19,:);
pi271 = in2(27,:);
pi291 = in2(29,:);
pi301 = in2(30,:);
pi371 = in2(37,:);
pi391 = in2(39,:);
pi401 = in2(40,:);
pi471 = in2(47,:);
pi481 = in2(48,:);
pi491 = in2(49,:);
pi501 = in2(50,:);
pi571 = in2(57,:);
pi581 = in2(58,:);
pi591 = in2(59,:);
pi601 = in2(60,:);
q2 = in1(2,:);
q3 = in1(3,:);
q4 = in1(4,:);
q5 = in1(5,:);
q6 = in1(6,:);
t2 = sin(1.57079632679);
t3 = cos(q3);
t4 = sin(q2);
t5 = cos(q2);
t6 = sin(q3);
t7 = t2.*t4.*(1.81e2./1.0e3);
t8 = t7-6.13e2./1.0e3;
t9 = t2.*t3.*t5.*(9.81e2./1.0e2);
t15 = t2.*t4.*t6.*(9.81e2./1.0e2);
t10 = t9-t15;
t11 = t2.*t5.*t6.*(1.81e2./1.0e3);
t12 = t2.*t5.*t6.*(9.81e2./1.0e2);
t13 = t2.*t3.*t4.*(9.81e2./1.0e2);
t14 = t12+t13;
t16 = cos(q4);
t17 = sin(q4);
t18 = t2.*t14.*t16;
t19 = t2.*t10.*t17;
t20 = t18+t19;
t21 = t3.*t8;
t22 = t6.*t8;
t23 = t2.*t3.*t5.*(1.81e2./1.0e3);
t24 = t22-t23;
t25 = t11+t21-5.71e2./1.0e3;
t26 = cos(q6);
t27 = cos(q5);
t28 = sin(q6);
t29 = t2.*t10.*t16;
t40 = t2.*t14.*t17;
t30 = t29-t40;
t31 = sin(q5);
t32 = t31.*(8.7e1./5.0e2);
t33 = t2.*t16.*t25;
t50 = t2.*t17.*t24;
t34 = t33-t50;
t35 = t27.*t34;
t36 = t32+t35;
t37 = t2.*t16.*t24;
t38 = t2.*t17.*t25;
t39 = t37+t38+3.0./2.5e1;
t41 = t2.*t3.*t4.*(1.81e2./1.0e3);
t42 = t11+t41;
t44 = t2.*t4.*t6.*(1.81e2./1.0e3);
t43 = t23-t44;
t45 = t2.*t16.*t42;
t46 = t2.*t17.*t43;
t47 = t45+t46;
t48 = t2.*t16.*t43;
t51 = t2.*t17.*t42;
t49 = t48-t51;
t52 = t27.*(8.7e1./5.0e2);
t68 = t31.*t34;
t53 = t52-t68;
t54 = t30.*t31.*t53;
t55 = t31.^2;
t56 = t20.*t28;
t72 = t26.*t27.*t30;
t57 = t56-t72;
t58 = t20.*t26;
t59 = t27.*t28.*t30;
t60 = t58+t59;
t61 = t37+t38;
t62 = t20.*t61;
t63 = t11+t21;
t64 = t27.^2;
t65 = t2.*t17.*t63;
t66 = t37+t65;
t67 = t27.*t30.*t36;
t69 = pi371.*t30;
t70 = pi391.*t20;
t71 = pi491.*t20;
t73 = pi591.*t60;
t74 = pi271.*t14;
t77 = t2.*t16.*t63;
t75 = t50-t77;
t76 = t30.*t34;
t78 = t28.*t30;
t79 = t20.*t26.*t27;
t80 = t78+t79;
t81 = t26.*t30;
t91 = t20.*t27.*t28;
t82 = t81-t91;
t83 = t28.*t36;
t84 = t26.*t39;
t85 = t83+t84;
t86 = t26.*t36;
t92 = t28.*t39;
t87 = t86-t92;
t88 = t57.*t87;
t89 = t20.*t55.*t66;
t90 = pi471.*t27.*t30;
t93 = t20.*t55.*t61;
G_vctr_sym = [0.0;t69+t70+t71+t73+t74+t90-pi291.*t10-pi571.*t57+pi301.*(-t10.*t42+t14.*t43+t10.*t63+t14.*(t22-t2.*t3.*t5.*(1.81e2./1.0e3)))-pi601.*(t54+t88+t80.*(t28.*t47-t26.*t27.*t49)+t82.*(t26.*t47+t27.*t28.*t49)-t60.*t85-t20.*t49.*t55)+pi401.*(t62+t76+t20.*t49-t30.*t47)+pi501.*(-t54+t62+t67-t30.*t47+t20.*t49.*t55+t20.*t49.*t64)+pi171.*t2.*t4.*(9.81e2./1.0e2)-pi191.*t2.*t5.*(9.81e2./1.0e2)-pi481.*t30.*t31-pi581.*t30.*t31;t69+t70+t71+t73+t74+t90-pi601.*(t54+t88+t89-t80.*(t28.*t75-t26.*t27.*t66)-t82.*(t26.*t75+t27.*t28.*t66)-t60.*t85)-pi291.*t10-pi571.*t57+pi401.*(t62+t76-t20.*t66+t30.*t75)-pi501.*(t54-t62-t67+t89-t30.*t75+t20.*t64.*t66)-pi481.*t30.*t31-pi581.*t30.*t31;t69+t70+t71+t73+t90-pi601.*(t54+t88+t93+t80.*(t28.*t34+t26.*t27.*t61)+t82.*(t26.*t34-t27.*t28.*t61)-t60.*t85)-pi571.*t57-pi501.*(t54-t62-t67+t76+t93+t20.*t61.*t64)-pi481.*t30.*t31-pi581.*t30.*t31;-pi601.*(-t20.*t31.*t36+t20.*t27.*t53-t26.*t53.*t80+t28.*t53.*t82+t20.*t26.*t31.*t87+t20.*t28.*t31.*t85)-pi471.*t20.*t31-pi481.*t20.*t27-pi581.*t20.*t27-pi571.*t20.*t26.*t31-pi591.*t20.*t28.*t31;pi571.*t82+pi591.*t80];