function Q = demo_rvbpcamv
warning('This demo is deprecated. See folder fa instead')

n = 50;
m = 2;
d = 1;

randn('state', 6);
rand('state', 6);

mu = [0;0];%20 * randn(m,1);
W = [1; 0.5];%orth(randn(m,d)) * diag(10:-1:(10-d+1));
X = orth(randn(d,n)')' * sqrt(n);
s2 = 0.1;

% Generate normal data
Y = W*X + repmat(mu,1,n);

% Add noise
Yn = Y + sqrt(s2)*randn(m,n);

% Generate some outliers
Yno = Yn;
p = (rand(m,n) < 0.05);
%p = zeros(m,n) == 1;
%p(1,1) = true;
%p(2,2) = true;
%p(1,3) = true;
%p(:,4) = true;
Yno(p) = 3 + 2 * rand(sum(p(:)),1);

% Generate some missing values
Ynom = Yno;
pmv = (rand(m,n) < 0.0);
Ynom(pmv) = NaN;

% Use STANDARD PCA for non-outlier data
Q = vbpcamv(Yn, d);
W_pca = Q.W
X_pca = Q.X
mu_pca = Q.mu
s2_pca = 1/Q.tau
%[W_pca, X_pca, mu_pca, s2_pca] = pca_full(Yn, d);
Y_pca = W_pca*X_pca + repmat(mu_pca, 1, n);

% Run different algorithms
Q = vbpcamv(Ynom, d);
W_p = Q.W
X_p = Q.X
mu_p = Q.mu
s2_p = 1/Q.tau
%[W_p, X_p, mu_p, s2_p] = pca_full(Ynom, d);
[W_rp,X_rp,Sv_rp,mu_rp,nu_rp,s2_rp,U_rp] = rppcamv(Ynom, d, 'maxiters', 100);
init.tau = 1e2;
init.nu = 1;
init.mu = 0;%mean(Yno,2)
prior = [];
% $$$ prior.atau = 1e3;
% $$$ prior.atau = 1e0
prior.aw = 1e-10;
prior.bw = 1e-3;
results_rvb = rvbpcamv(Ynom, d, 'maxiters', 200, 'init', init, 'prior', ...
                       prior, 'startupdatehyper', 10, 'startrotate', 1, ...
                       'rotate', true);
W_rvb = results_rvb.W;
X_rvb = results_rvb.X;
S_rvb = results_rvb.Sv;
mu_rvb = results_rvb.mu;
nu_rvb = results_rvb.nu;

% Reconstruct
Y_p = W_p*X_p + repmat(mu_p,1,n);
Y_rp = W_rp*X_rp + repmat(mu_rp,1,n);
Y_rvb = W_rvb*X_rvb + repmat(mu_rvb,1,n);

% Error of the principal subspace
err_W_p = 180 * subspace(W_p, W_pca) / pi
err_W_rp = 180 * subspace(W_rp, W_pca) / pi
err_W_rvb = 180 * subspace(W_rvb, W_pca) / pi

plot_results(Ynom, []);
plot_results(Ynom, Y_p)
plot_results(Ynom, Y_rp)
plot_results(Ynom, Y_rvb)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%55
function plot_results(Y, Yh)
%subspace2d(Yh, [], Y)
figure
plot(Y(1,:), Y(2,:), 'x', 'markersize', 5, 'color', [.2 .2 .2]);
if ~isempty(Yh)
  hold on
  plot(Yh(1,:), Yh(2,:), 'ko', 'markersize', 3);
  nans = nan*ones(1,size(Y,2));
  plot([Y(1,:);Yh(1,:);nans], [Y(2,:);Yh(2,:);nans], ':', 'color', [.0 .0 .0]);
end
set(gca, 'DataAspectRatioMode', 'manual');
set(gca, 'DataAspectRatio', [1 1 1]);
set(gca, 'XTick', [], 'YTick', []);
set(gcf, 'units', 'centimeters');
pos = get(gcf, 'position');
set(gcf, 'position', [pos(1), pos(2), 5 5])
set(gcf, 'PaperPositionMode', 'auto', 'paperunits', 'centimeters', 'PaperSize', [5 5]);

xmg = 0.05 * (max(Y(1,:)) - min(Y(1,:)));
ymg = 0.05 * (max(Y(2,:)) - min(Y(2,:)));
xl = [min(Y(1,:))-xmg, max(Y(1,:))+xmg];
yl = [min(Y(2,:))-ymg, max(Y(2,:))+ymg];
set(gca, 'xlim', xl, 'ylim', yl);

% $$$ % Mark outliers
% $$$ hold on
% $$$ indeces = sum(p,1)>0;
% $$$ plot(VYno(1,indeces), VYno(2, indeces), 'go', 'MarkerSize', 8);

% $$$ % Mark observations with missing values
% $$$ indeces = sum(pmv,1)>0;
% $$$ scatter(VYno(1,indeces), VYno(2, indeces), 'yo');

return


function [ a, l, r ] = i4vec_part_quick_a ( n, a )

%*****************************************************************************80
%
%% I4VEC_PART_QUICK_A reorders an I4VEC as part of a quick sort.
%
%  Discussion:
%
%    The routine reorders the entries of A.  Using A(1) as a key,
%    all entries of A that are less than or equal to the key will
%    precede the key which precedes all entries that are greater than the key.
%
%  Example:
%
%    Input:
%
%      N = 8
%
%      A = ( 6, 7, 3, 1, 6, 8, 2, 9 )
%
%    Output:
%
%      L = 3, R = 6
%
%      A = ( 3, 1, 2, 6, 6, 8, 9, 7 )
%            -------        -------
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 February 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries of A.
%
%    Input, integer A(N), the array to be checked.
%
%    Output, integer A(N), has been reordered as described above.
%
%    Output, integer L, R, the indices of A that define the three segments.
%    Let KEY = the input value of A(1).  Then
%    I <= L                 A(I) < KEY;
%         L < I < R         A(I) = KEY;
%                 R <= I    KEY < A(I).
%
  if ( n < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'I4VEC_PART_QUICK_A - Fatal error!\n' );
    fprintf ( 1, '  N < 1.\n' );
    error ( 'I4VEC_PART_QUICK_A - Fatal error!' );
  elseif ( n == 1 )
    l = 0;
    r = 2;
    return;
  end

  key = a(1);
  m = 1;
%
%  The elements of unknown size have indices between L+1 and R-1.
%
  l = 1;
  r = n + 1;

  for i = 2 : n

    if ( key < a(l+1) )
      r = r - 1;
      [ a(r), a(l+1) ] = i4_swap ( a(r), a(l+1) );
    elseif ( a(l+1) == key )
      m = m + 1;
      [ a(m), a(l+1) ] = i4_swap ( a(m), a(l+1) );
      l = l + 1;
    elseif ( a(l+1) < key )
      l = l + 1;
    end

  end
%
%  Now shift small elements to the left, and KEY elements to center.
%
  for i = 1 : l - m
    a(i) = a(i+m);
  end
%
%  Out of bounds here, occasionally.
%
  l = l - m;

  a(l+1:l+m) = key;

  return
end


function [divTr, divTe]= sample_KFold(label, folds, varargin)
%SAMPLE_KFOLD - Sampling function: random divisions (by default stratified)
%
%Synopsis:
%  [PARTR, PARTE]= sample_KFold(LABEL, FOLDS, <OPT>)
%
%Arguments:
% LABEL  - class label of size [nClasses x nSamples].
% FOLDS  - DOUBLE nFolds: number of folds into which the samples are
%          divided. Or FOLDS can be [nShifts nFolds] in which case
%          all partitions will also be generated in shifted versions.
% OPT    - property/value list of optinal parameters:
%   'stratified' [BOOL] stratified sampling (true, default)
%          or completely random sampling (false).
%
%Returns:
% DIVTR   - Partitions of the training set
%           DIVTR{n}: cell array holding the training sets folds for
%           shuffle #n, more specificially
%           DIVTR{n}{m} holds the indices of the training set of the m-th
%           fold of shuffle #n
% DIVTE   - analogue to DIVTR, for the test sets

% 2014-02 Martijn Schreuder

props = {'Stratified'      true          'BOOL|DOUBLE[1]'
        };

if nargin==0,
  divTr= props;
  return;
end

opt= opt_proplistToStruct(varargin{:});
[opt,~] = opt_setDefaults(opt, props, 1);

misc_checkType(label, 'DOUBLE[- -]');
misc_checkType(folds, 'DOUBLE|DOUBLE[2]');

nSamples = sum(label,2);

if length(folds)==1
  folds= [1 folds];
end

% check that the number of folds is smaller than the smallest class
if any(folds(2) > nSamples),
    error('The number of folds is larger than the number of samples in the smallest class');
end

%divTr= {cell(1,folds(2))};
%divTe= {cell(1,folds(2))};
for nn= 1:folds(1)
  clear idx;
  % prepare indices
  if opt.Stratified
      for cl = 1:length(nSamples)
          clid = find(label(cl,:));
          idx{cl} = clid(randperm(nSamples(cl)));
      end
  else
      idx{1} = randperm(sum(nSamples));
  end

  % make divisions
  for cl = 1:length(idx)
      div{cl}= round(linspace(0, nSamples(cl), folds(2)+1));
  end

  % sample
  for kk= 1:folds(2)
    divTe{nn}{kk} = [];
    for cl = 1:length(div),
      divTe{nn}{kk}= sort([divTe{nn}{kk} idx{cl}(div{cl}(kk)+1:div{cl}(kk+1))]);
    end
    divTr{nn}{kk}= setdiff(1:sum(nSamples), divTe{nn}{kk});

    % check that all classes are inhabited
    if ~all(sum(label(:,divTr{nn}{kk}))),
      error('empty classes in training set');
    end
  end
end


function value = r4_cos ( x )

%*****************************************************************************80
%
%% R4_COS evaluates the cosine of an R4 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the cosine of X.
%

  persistent ntsn
  persistent pi2rec
  persistent pihi
  persistent pilo
  persistent pirec
  persistent sincs
  persistent xmax
  persistent xsml
  persistent xwarn
%
%  pihi + pilo = pi.  pihi is exactly representable on all machines
%  with at least 8 bits of precision.  whether it is exactly
%  represented depends on the compiler.  this routine is more
%  accurate if it is exactly represented.
%
  pi2 = 1.57079632679489661923;
  pi2rec = 0.636619772367581343;
  pihi = 3.140625;
  pilo = 9.6765358979323846E-04;
  pirec = 0.31830988618379067;

  if ( isempty ( ntsn ) )

    sincs = [ ...
      -0.374991154955873175840, ...
      -0.181603155237250201864, ...
      +0.005804709274598633559, ...
      -0.000086954311779340757, ...
      +0.000000754370148088851, ...
      -0.000000004267129665056, ...
      +0.000000000016980422945, ...
      -0.000000000000050120579, ...
      +0.000000000000000114101, ...
      -0.000000000000000000206 ]';

    ntsn = r4_inits ( sincs, 10, 0.1 * r4_mach ( 3 ) );
    xsml = sqrt ( 2.0 * r4_mach ( 3 ) );
    xmax = 1.0 / r4_mach ( 4 );
    xwarn = sqrt ( xmax );

  end

  absx = abs ( x );
  y = absx + pi2;

  if ( xmax < y )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_COS - Warning!\n' );
    fprintf ( 1, '  No precision because |X| is big.\n' );
    value = 0.0;
    return
  end

  if ( xwarn < y )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_COS - Warning!\n' );
    v '  Answer < half precision because |X| is big.\n' );
  end

  value = 1.0;

  if ( absx < xsml )
    return
  end

  xn = r4_aint ( y * pirec + 0.5 );
  n2 = r4_aint ( mod ( xn, 2.0 ) + 0.5 );
  xn = xn - 0.5;
  f = ( absx - xn * pihi ) - xn * pilo;

  xn = 2.0 * ( f * pi2rec ) * ( f * pi2rec ) - 1.0;
  value = f + f * r4_csevl ( xn, sincs, ntsn );

  if ( n2 ~= 0 )
    value = - value;
  end

  if ( value < - 1.0 )
    value = - 1.0;
  elseif ( 1.0 < value )
    value = + 1.0;
  end

  return
end


function jed = cws_to_jed_gps ( c, w, s )

%*****************************************************************************80
%
%% CWS_TO_JED_GPS converts a GPS CWS date to a JED.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer C, integer W, real S,
%    the GPS cycle/week/second date.
%
%    Output, real JED, the corresponding Julian Ephemeris Date.
%
  jed_epoch = epoch_to_jed_gps ( );

  d = ( 7 * ( 1024 * c + w ) ) + s / ( 24.0 * 60.0 * 60.0 );

  jed = jed_epoch + d;

  return
end


function tests = test_spm_ncTpdf
% Unit Tests for spm_ncTpdf
%__________________________________________________________________________
% Copyright (C) 2018 Wellcome Trust Centre for Neuroimaging

% $Id: test_spm_ncTpdf.m 7260 2018-02-19 10:55:53Z guillaume $

tests = functiontests(localfunctions);

function test_spm_ncTpdf_1(testCase)
exp = spm_Tpdf(0,1);
act = spm_ncTpdf(0,1,0);
tol = 1e-12;
testCase.verifyEqual(act, exp,'AbsTol',tol);

exp = spm_Tpdf(-2:0.5:2,2);
act = spm_ncTpdf(-2:0.5:2,2,0);
tol = 1e-12;
testCase.verifyEqual(act, exp,'AbsTol',tol);

exp = spm_Tpdf(0,1:4);
act = spm_ncTpdf(0,1:4,0);
tol = 1e-12;
testCase.verifyEqual(act, exp,'AbsTol',tol);

exp = spm_Tpdf(-2:2,1:5);
act = spm_ncTpdf(-2:2,1:5,0);
tol = 1e-12;
testCase.verifyEqual(act, exp,'AbsTol',tol);

function test_spm_ncTpdf_2(testCase)
exp = 0.193064705260108; % nctpdf(0,1,1)
act = spm_ncTpdf(0,1,1);
tol = 1e-12;
testCase.verifyEqual(act, exp,'AbsTol',tol);

exp = 2.072225240523640e-04; % nctpdf(1,10,5);
act = spm_ncTpdf(1,10,5);
tol = 1e-12;
testCase.verifyEqual(act, exp,'AbsTol',tol);

exp = [... % nctpdf(-1:3,6:10,-2:2);
   0.240403340723731
   0.233509118657019
   0.227607580145303
   0.223756811547761
   0.219731001980809]';
act = spm_ncTpdf(-1:3,6:10,-2:2);
tol = 1e-12;
testCase.verifyEqual(act, exp,'AbsTol',tol);


function T = imageStats5(f)
%imageStats5 Sample function used in Chapter 2.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt
P = size(f,3);
% Initialize table variables to hold information for P images.
ArraySize = zeros(P,2);
GlobalMean = zeros(P,1);
RowMeans = zeros(P,size(f,1));
ColumnMeans = zeros(P,size(f,2));
for k = 1:P
   fk = f(:,:,k);
   ArraySize(k,:) = size(fk);
   GlobalMean(k) = mean2(fk);
   RowMeans(k,:) = mean(fk,2)'; % Transpose to store row means as a row
                                % vector
   ColumnMeans(k,:) = mean(fk,1);
end
% Create the table from the individual variables.
T = table(ArraySize,GlobalMean,RowMeans,ColumnMeans);


function a = sym2polys(p,x)

%Sym2Polys  Extract the coefficients of a symbolic polynomial.
%           This function is an extension of the Matlab SYM2POLY and COEFFS
%           functions in that it allows the coefficients to be symbolic and
%           returns the full coefficient vector including the zero coefficients.
%
%Usage: c = sym2polys(p,x)
%       where p is the (multi) symbolic polynomial and x is the
%       independent variable. If x is not specified then the variable
%       alphabetically closest to x is used as the independent variable.
%
%Example:    If p = a*b*x^3 + b*c*x + c*d
%            then sym2polys(p) returns [a*b, 0, b*c, c*d]
%            whereas sym2polys(p,'b') returns [a*x^3+c*x, c*d]
%            Note that coeffs(p,x) returns [c*d, b*c, a*b]

%see also: sym2poly, coeffs

% Mukhtar Ullah
% mukhtar.ullah@informatik.uni-rostock.de
% September 2, 2004

if nargin == 1, x = findsym(p,1); end

[c,t] = coeffs(p,x);
i = sym2poly(sort(sum(t*(1:numel(t)).')));
a = sym(i);
a(i>0) = c(i(i>0));

if isempty(findsym(a)), a = double(a); end


function [H,g,Aineq,bineq,Aeq,beq] = fcn_get_QP_form_eta(Xt,Ut,Xd,Ud,p)
% min. 0.5 * x' * H *x + g' * x
% s.t. Aineq *x <= bineq
%      Aeq * x <= beq
% X = [pc dpc vR wb pf]': [30,1]
% q = [pc dpc eta wb]: [12 1]
% lb/ub - [4,n_hor]

%% parameters
mu = p.mu;
n_hor = p.predHorizon;
Umax = p.Umax;
decayRate = p.decayRate;

R = p.R;
Q = p.Q;
Qf = p.Qf;
[Qx,Qv,Qeta,Qw] = deal(Q(1:3,1:3),Q(4:6,4:6),Q(7:9,7:9),Q(10:12,10:12));
[Qxf,Qvf,Qetaf,Qwf] = deal(Qf(1:3,1:3),Qf(4:6,4:6),Qf(7:9,7:9),Qf(10:12,10:12));

nX = 12;
nU = 12;

%%%%%%% A,B,d matrices for linear dynamics %%%%%%%%%%%
[A,B,d] = fcn_get_ABD_eta(Xt,Ut,p);

%% Decompose
Rt = reshape(Xt(7:15,1),[3,3]);
qt = [Xt(1:6);[0;0;0];Xt(16:18)];

% lb <= Fz <= ub
Fzd = Ud([3 6 9 12],:);
lb = -1 * Fzd;
ub = 2 * Fzd;

%% Matrices for QP
H = zeros((nX + nU) * n_hor);
g = zeros(size(H,1),1);
Aeq = zeros(nX * n_hor,(nX+nU) * n_hor);
beq = zeros(size(Aeq,1),1);
if p.gait == -2
    Aineq_unit = [1 0 0;-1 0 0;0 1 0;0 -1 0;0 0 1;0 0 -1];
else
    Aineq_unit = [1 0 -mu;-1 0 -mu;0 1 -mu;0 -1 -mu;0 0 1; 0 0 -1];
end
nAineq_unit = size(Aineq_unit,1);
Aineq = zeros(4*nAineq_unit*n_hor,(nX+nU)*n_hor);
bineq = zeros(size(Aineq,1),1);
for i_hor = 1:n_hor
    xd = Xd(1:3,i_hor);
    vd = Xd(4:6,i_hor);
    Rd = reshape(Xd(7:15,i_hor),[3,3]);
    wd = Xd(16:18,i_hor);

    %% Objective function
    idx_u = (i_hor-1) * (nX + nU) + (1:nU);
    idx_x = (i_hor-1) * (nX + nU) + nU + (1:nX);
    if i_hor == n_hor
        H(idx_x,idx_x) = Qf * decayRate^(i_hor-1);
        g(idx_x) = [-Qxf * xd;
                    -Qvf * vd;
                     Qetaf * veeMap(logm(Rd' * Rt));
                    -Qwf * wd] * decayRate^(i_hor-1);
    else
        H(idx_x,idx_x) = Q * decayRate^(i_hor-1);
        g(idx_x) = [-Qx * xd;
                    -Qv * vd;
                     Qeta * veeMap(logm(Rd' * Rt));
                    -Qw * wd] * decayRate^(i_hor-1);
    end
    H(idx_u,idx_u) = R * decayRate^(i_hor-1);
    g(idx_u) = R' * (Ut - Ud(:,i_hor)) * decayRate^(i_hor-1);

    %% Equality constraints
    if i_hor == 1
        Aeq(1:nX,1:(nU+nX)) = [-B,eye(nX)];
        beq(1:nX) = A * qt + d;
    else
        Aeq((i_hor-1)*nX+(1:nX),(i_hor-2)*(nX+nU)+nU+(1:(2*nX+nU)))= [-A -B eye(nX)];
        beq((i_hor-1)*nX+(1:nX)) = d;
    end

    %% Inequality constraints
    Fi = zeros(4*nAineq_unit,12);
    hi = zeros(size(Fi,1),1);
    for i_leg = 1:4
        idx_F = (i_leg-1)*nAineq_unit + (1:nAineq_unit);
        idx_u = (i_leg-1)*3 + (1:3);
        Fi(idx_F,idx_u) = Aineq_unit;
        if p.gait == -2
            hi(idx_F) = [Umax-Ut(idx_u(1));Umax+Ut(idx_u(1));...
                         Umax-Ut(idx_u(2));Umax+Ut(idx_u(2));...
                         Umax-Ut(idx_u(3));Umax+Ut(idx_u(3))];
        else
            hi(idx_F) = [mu*Ut(idx_u(3))-Ut(idx_u(1));
                         mu*Ut(idx_u(3))+Ut(idx_u(1));
                         mu*Ut(idx_u(3))-Ut(idx_u(2));
                         mu*Ut(idx_u(3))+Ut(idx_u(2));
                         ub(i_leg,i_hor)-Ut(idx_u(3))+Ud(idx_u(3),i_hor);
                        -lb(i_leg,i_hor)+Ut(idx_u(3))-Ud(idx_u(3),i_hor)];
        end
    end
    idx_A = (i_hor-1) * 4*nAineq_unit + (1:4*nAineq_unit);
    idx_z = (i_hor-1) * (nX+nU) + (1:nU);
    Aineq(idx_A,idx_z) = Fi;
    bineq(idx_A) = hi;
end

end


function x2d = project(x3d, K, RT)

P = K * RT;

x2d = P*[x3d ones(size(x3d,1), 1)]';
x2d(1,:) = x2d(1,:) ./ x2d(3,:);
x2d(2,:) = x2d(2,:) ./ x2d(3,:);
x2d = x2d(1:2,:)';


function d = det2x2(x)

% DET2X2 computes determinant of matrix x, using explicit analytic definition
% if size(x,1) < 4, otherwise use MATLAB det-function

% Copyright (C) 2012, Donders Centre for Cognitive Neuroimaging, Nijmegen, NL
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip 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.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

siz = size(x);
if all(siz(1:2)==2),
  d = x(1,1,:,:).*x(2,2,:,:) - x(1,2,:,:).*x(2,1,:,:);
elseif all(siz(1:2)==3),
  d = x(1,1,:,:).*x(2,2,:,:).*x(3,3,:,:) - ...
      x(1,1,:,:).*x(2,3,:,:).*x(3,2,:,:) - ...
      x(1,2,:,:).*x(2,1,:,:).*x(3,3,:,:) + ...
      x(1,2,:,:).*x(2,3,:,:).*x(3,1,:,:) + ...
      x(1,3,:,:).*x(2,1,:,:).*x(3,2,:,:) - ...
      x(1,3,:,:).*x(2,2,:,:).*x(3,1,:,:);
elseif numel(siz)==2,
  d = det(x);
else
  ft_error('not implemented');
  % write for loop for the higher dimensions, using normal inv
end


%
sz = [42,31];
res = 3;
j = 2;
min_margin = 2^j*[1,1];

% the smallest multiple of 2^res that is larger than sz by at least 2*min_margin
sz_paded = 2^res*ceil((sz + 2*min_margin)/2^res)


function [anim,HEye] = setFirstPRtToId( anim )
% Sets the first P or (R,t) of an Animation to eye(3,4)
%
% The function changes all the S and SBasis accordingly to preserve the
% projections.
%
% if R and t is defined, the rigid transform H is computed such that
%   [R(:,:,1),t(:,1)]*H=eye(3,4)
%   H is of the form [rotation,translation; 0 0 0 1]
%
% if R is not defined but P is, the homography H is computed such that:
%   P(:,:,1)*H=eye(3,4)
%
% USAGE
%  anim = anim.setFirstRToId()
%
% INPUTS
%  anim     - Animation object (help Animation for details)
%
% OUTPUTS
%  anim     - modified Animation
%  HEye     - [3x4] rigid transform if (R,t) are defined, or [3x4]
%             projective transform is P is defined
%
% EXAMPLE
%
% See also GENERATETOYANIMATION
%
% Vincent's Structure From Motion Toolbox      Version 3.1.1
% Copyright (C) 2008-2011 Vincent Rabaud.  [vrabaud-at-cs.ucsd.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the GPL [see external/gpl.txt]

isTorresaniModel = anim.nBasis==size(anim.l,1)+1;
if isempty(anim.R)
  HEye=anim.P(:,:,1)\eye(3,3);
  [disc,disc,V]=svd(anim.P(:,:,1));
  HEye(:,4)=V(:,4);
  % apply the homography to P
  anim.P=multiTimes(anim.P,HEye,1);
else
  HEye=[anim.R(:,:,1)',zeros(3,1);0,0,0,1];
  if anim.nBasis~=0 || isTorresaniModel
    HEye(1:3,4)=-anim.R(:,:,1)'*anim.t(:,1);
    % Re-generate the rotations and translations
    anim=subsasgn(anim,struct('type','.','subs','t'),...
      bsxfun(@minus,anim.t,reshape(multiTimes(anim.R, ...
      anim.R(:,:,1)'*anim.t(:,1),1),3,anim.nFrame)));
  end
  anim=subsasgn(anim,struct('type','.','subs','R'),...
    multiTimes(anim.R,anim.R(:,:,1)',1));
end

if anim.nBasis~=0
  % Re-generate the basis
  invHEye = inv(HEye);
  if isTorresaniModel
    SBasis=multiTimes(invHEye(1:3,1:3),anim.SBasis,1.2);
    SBasis(:,:,1)=bsxfun(@plus,SBasis(:,:,1),invHEye(1:3,4));
  else
    SBasis=multiTimes(invHEye(1:3,1:3),anim.SBasis,1.2);
  end

  % use subsasgn so that S is modified at the same time too
  anim=subsasgn(anim,struct('type','.','subs','SBasis'),SBasis);
else
  anim.S=normalizePoint(multiTimes(inv(HEye),normalizePoint(anim.S,-4),1.2),4);
end


function BiObj = Estimation(PopObj,r)
% Estimate the proximity and crowding degree of each solution

%------------------------------- 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".
%--------------------------------------------------------------------------

    N = size(PopObj,1);

    %% Proximity estimation
    fmax   = repmat(max(PopObj,[],1),N,1);
    fmin   = repmat(min(PopObj,[],1),N,1);
    PopObj = (PopObj-fmin)./(fmax-fmin);
    fpr    = sum(PopObj,2);

    %% Crowding degree estimation
    d     = pdist2(PopObj,PopObj);
    d(logical(eye(length(d)))) = inf;
    fprm  = repmat(fpr,1,N);
    case1 = d<r & fprm<=fprm';
    case2 = d<r & fprm>fprm';
    sh        = zeros(N);
    sh(case1) = (0.5*(1-d(case1)/r)).^2;
    sh(case2) = (1.5*(1-d(case2)/r)).^2;
    fcd   = sqrt(sum(sh,2));
    BiObj = [fpr,fcd];
end


function coeffs = decdemo( im, option )
% DECDEMO  demonstrates nonsubsampled Contourlet decomposition and reconstruction.
%
%   DECDEMO shows how to use the nonsubsampled contourlet toolbox to decompose
%   and reconstruct an image.  It provides a sample script that uses
%   basic functions such as nsctdec, nsctrec, and shownsct.
%
%   It can be modified for applications such as image analysis,
%   image retrieval and image processing.
%
% Input:
%	image:  a double or integer matrix for the input image.
%           The default is the zoneplate image.
%   option: option for the demos. The default value is 'auto'
%       'auto' ------  automtatical demo, no input
%       'user' ------  semi-automatic demo, simple interactive inputs
%       'expert' ----  mannual, complete interactive inputs.
%                      (Not implmented in this version)
%
% Output:
%	coeffs: a cell vector for the contourlet decomposition coefficients.
%
% See also:     NSCTDEC, NSCTREC, SHOWNSCT.

% History:
%   08/08/2004  Created by Jianping Zhou.

disp('Welcome to the nonsubsampled Contourlet decomposition demo! :)');
disp('Type help decdemo for help' ) ;
disp('You can also view decdemo.m for details.') ;
disp(' ');

% Input image
if ~exist('im', 'var')
    % Zoneplate image: good for illustrating multiscale and directional
    % decomposition
    im = imread ('zoneplate.png') ;
elseif isstr(im)
    im = imread ( im ) ;
else
    error('You shall input valid image name!');
end

% Show the input image
disp( 'Displaying the input image...');
clf;
imagesc(im, [0, 255]);
title( 'Input image' ) ;
axis image off;
colormap(gray);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Image decomposition by nonsubsampled contourlet transform (NSCT).
% This is the iterated filter bank that computes the nonsubsampled
% contourlet transform.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Parameteters:
nlevels = [0, 1, 3] ;        % Decomposition level
pfilter = 'maxflat' ;              % Pyramidal filter
dfilter = 'dmaxflat7' ;              % Directional filter

% Nonsubsampled Contourlet decomposition
coeffs = nsctdec( double(im), nlevels, dfilter, pfilter );

% Display the coefficients
disp('Displaying the contourlet coefficients...') ;
shownsct( coeffs ) ;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Nonsubsampled Contourlet transform (NSCT) reconstruction.
% This is the inverse of nsctdec, i.e.
% imrec = nsctrec(coeffs, dfilter, pfilter);
% would reconstruct imrec = im
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Reconstruct image
imrec = nsctrec( coeffs, dfilter, pfilter ) ;

disp('Displaying the reconstructed image...') ;
disp('It should be a perfect reconstruction' ) ;
disp(' ') ;

% Show the reconstruction image and the original image
figure;
subplot(1,2,1), imagesc( im, [0, 255] );
title('Original image' ) ;
colormap(gray);
axis image off;
subplot(1,2,2), imagesc( imrec, [0, 255] );
title('Reconstructed image' ) ;
colormap(gray);
axis image off;

mse = sum( sum( (imrec - double(im)).^2 ) );
mse = mse / prod(size(im));

disp( sprintf('The mean square error is: %f', mse ) );
disp(' ');


function [L,S] = RPCA(X)
[n1,n2] = size(X);
mu = n1*n2/(4*sum(abs(X(:))));
lambda = 1/sqrt(max(n1,n2));
thresh = 1e-7*norm(X,'fro');

S = zeros(size(X));
Y = zeros(size(X));
count = 0;
while((norm(X-L-S,'fro')>thresh)&&(count<1000))
    L = SVT(X-S+(1/mu)*Y,1/mu);
    S = shrink(X-L+(1/mu)*Y,lambda/mu);
    Y = Y + mu*(X-L-S);
    count = count + 1
end


function [model,centers] = FCMmodel(Problem,Population,L1,L2)
% Fuzzy clustering-based method for modeling c_size* M models, where c_size
% is the number of clusters and M the number of objectives.

%------------------------------- 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".
%--------------------------------------------------------------------------

% This function is written by Cheng He

    PopDec      = Population.decs;
    csize       = 1 + ceil((length(Population)-L1)/L2);
    [centers,~] = fcm(PopDec,csize,[2 NaN 0.05 false]);
    dis         = pdist2(PopDec,centers);
    [~,index]   = sort(-dis);
    group       = index(1:L1,:);

    %% Build GP model of each objective for each cluster
    model   = cell(csize,Problem.M);
    THETA   = 5.*ones(csize,Problem.M,Problem.D);
    for i   = 1 : csize
        temp = Population(group(:,i));
        PopDec = temp.decs;
        PopObj = temp.objs;
        for j = 1 : Problem.M
            dmodel = dacefit(PopDec,PopObj(:,j),...
                     'regpoly0','corrgauss',...
                     squeeze(THETA(i,j,:)),...
                     1e-5.*ones(1,Problem.D),...
                     100.*ones(1,Problem.D));
            model{i,j}   = dmodel;
            THETA(i,j,:) = dmodel.theta;
        end
    end
end


function [L,a,b] =rgb2lab(R,G,B)
% function [L, a, b] = RGB2Lab(R, G, B)
% RGB2Lab takes matrices corresponding to Red, Green, and Blue, and
% transforms them into CIELab.  This transform is based on ITU-R
% Recommendation  BT.709 using the D65 white point reference.
% The error in transforming RGB -> Lab -> RGB is approximately
% 10^-5.  RGB values can be either between 0 and 1 or between 0 and 255.
% By Mark Ruzon from C code by Yossi Rubner, 23 September 1997.
% Updated for MATLAB 5 28 January 1998.

if (nargin == 1)
  B = double(R(:,:,3));
  G = double(R(:,:,2));
  R = double(R(:,:,1));
end

if ((max(max(R)) > 1.0) | (max(max(G)) > 1.0) | (max(max(B)) > 1.0))
  R = R/255;
  G = G/255;
  B = B/255;
end

[M, N] = size(R);
s = M*N;

% Set a threshold
T = 0.008856;

RGB = [reshape(R,1,s); reshape(G,1,s); reshape(B,1,s)];

% RGB to XYZ
MAT = [0.412453 0.357580 0.180423;
       0.212671 0.715160 0.072169;
       0.019334 0.119193 0.950227];
XYZ = MAT * RGB;

X = XYZ(1,:) / 0.950456;
Y = XYZ(2,:);
Z = XYZ(3,:) / 1.088754;

XT = X > T;
YT = Y > T;
ZT = Z > T;

fX = XT .* X.^(1/3) + (~XT) .* (7.787 .* X + 16/116);

% Compute L
Y3 = Y.^(1/3);
fY = YT .* Y3 + (~YT) .* (7.787 .* Y + 16/116);
L  = YT .* (116 * Y3 - 16.0) + (~YT) .* (903.3 * Y);

fZ = ZT .* Z.^(1/3) + (~ZT) .* (7.787 .* Z + 16/116);

% Compute a and b
a = 500 * (fX - fY);
b = 200 * (fY - fZ);

L = reshape(L, M, N);
a = reshape(a, M, N);
b = reshape(b, M, N);

if ((nargout == 1) | (nargout == 0))
  L = cat(3,L,a,b);
end


function ns = count_squares(I,x1,y1,x2,y2,win);

[ny,nx] = size(I);

if ((x1-win <= 0) || (x1+win >= nx) || (y1-win <= 0) || (y1+win >= ny) || ...
        (x2-win <= 0) || (x2+win >= nx) || (y2-win <= 0) || (y2+win >= ny))
    ns = -1;
    return;
end;

if ((x1 - x2)^2+(y1-y2)^2) <  win,
    ns = -1;
    return;
end;

lambda = [y1 - y2;x2 - x1;x1*y2 - x2*y1];
lambda = 1/sqrt(lambda(1)^2 + lambda(2)^2) * lambda;
l1 = lambda + [0;0;win];
l2 = lambda - [0;0;win];
dx = x2-x1;
dy = y2 - y1;

if abs(dx) > abs(dy),
   if x2 > x1,
      xs = x1:x2;
   else
      xs = x1:-1:x2;
   end;
   ys = -(lambda(3) + lambda(1)*xs)/lambda(2);
else
   if y2 > y1,
       ys = y1:y2;
   else
       ys = y1:-1:y2;
   end;
   xs = -(lambda(3) + lambda(2)*ys)/lambda(1);
end;

Np = length(xs);
xs_mat = ones(2*win + 1,1)*xs;
ys_mat = ones(2*win + 1,1)*ys;
win_mat = (-win:win)'*ones(1,Np);
xs_mat2 = round(xs_mat - win_mat * lambda(1));
ys_mat2 = round(ys_mat - win_mat * lambda(2));
ind_mat = (xs_mat2 - 1) * ny + ys_mat2;
ima_patch = zeros(2*win + 1,Np);
ima_patch(:) = I(ind_mat(:));

%ima2 = ima_patch(:,win+1:end-win);

filtk = [ones(win,Np);zeros(1,Np);-ones(win,Np)];
out_f = sum(filtk.*ima_patch);
out_f_f = conv2(out_f,[1/4 1/2 1/4],'same');
out_f_f = out_f_f(win+1:end-win);
ns = length(find(((out_f_f(2:end)>=0)&(out_f_f(1:end-1)<0)) | ((out_f_f(2:end)<=0)&(out_f_f(1:end-1)>0))))+1;

return;


function table2 = r8mat_border_add ( m, n, table )

%*****************************************************************************80
%
%% R8MAT_BORDER_ADD adds a "border" to an R8MAT.
%
%  Discussion:
%
%    An R8MAT is an array of R8's.
%
%    We suppose the input data gives values of a quantity on nodes
%    in the interior of a 2D grid, and we wish to create a new table
%    with additional positions for the nodes that would be on the
%    border of the 2D grid.
%
%                  0 0 0 0 0 0
%      * * * *     0 * * * * 0
%      * * * * --> 0 * * * * 0
%      * * * *     0 * * * * 0
%                  0 0 0 0 0 0
%
%    The illustration suggests the situation in which a 3 by 4 array
%    is input, and a 5 by 6 array is to be output.
%
%    The old data is shifted to its correct positions in the new array.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 January 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real TABLE(M,N), the table data.
%
%    Output, real TABLE2(M+2,N+2), the augmented table data.
%
  table2 = zeros(m+2,n+2);

  table2(1,1:n+2) = 0.0;
  table2(m+2,1:n+2) = 0.0;
  table2(2:m+1,1) = 0.0;
  table2(2:m+1,n+2) = 0.0;

  table2(2:m+1,2:n+1) = table(1:m,1:n);

  return
end


%   AUTHORSHIP
%   Math Lead & Primary Developer:  Connor Meehan <connor.gw.meehan@gmail.com>
%   Secondary Developer: Stephen Meehan <swmeehan@stanford.edu>
%   Bioinformatics Lead:  Wayne Moore <wmoore@stanford.edu>
%   Funded by the Herzenberg Lab at Stanford University
%   License: BSD 3 clause
%
function [rows, cols, vals] = compute_membership_strengths(knn_indices, knn_dists, sigmas, rhos, same_set)
%COMPUTE_MEMBERSHIP_STRENGTHS Construct the membership strength data for
% the 1-skeleton of each local fuzzy simplicial set -- this is formed as a
% sparse matrix where each row is a local fuzzy simplicial set, with a
% membership strength for the 1-simplex to each other data point.
%
% [rows, cols, vals] = COMPUTE_MEMBERSHIP_STRENGTHS(knn_indices, knn_dists, sigmas, rhos, same_set)
%
% Parameters
% ----------
% knn_indices: array of size (n_samples, n_neighbors)
%     The indices on the "n_neighbors" closest points in the dataset.
%
% knn_dists: array of size (n_samples, n_neighbors)
%     The distances to the "n_neighbors" closest points in the dataset.
%
% sigmas: array of size (n_samples, 1)
%     The normalization factor derived from the metric tensor approximation.
%
% rhos: array of size (n_samples, 1)
%     The local connectivity adjustment.
%
% Returns
% -------
% rows: array of size (n_samples*n_neighbors, 1)
%     Row data for the resulting sparse matrix.
%
% cols: array of size (n_samples*n_neighbors, 1)
%     Column data for the resulting sparse matrix.
%
% vals: array of size (n_samples*n_neighbors, 1)
%     Entries for the resulting sparse matrix.

    if nargin < 5
        same_set = true;
    end

    [n_samples, n_neighbors] = size(knn_indices);

    knn_fail = knn_indices == -1;

    rows = repmat((1:n_samples)', 1, n_neighbors);
    rows(knn_fail) = NaN;
    rows = rows';
    rows = rows(:);

    cols = knn_indices;
    cols(knn_fail) = NaN;
    cols = cols';
    cols = cols(:);

    d = knn_dists - rhos;

    vals = exp(-max(0, d./repmat(sigmas, [1 n_neighbors])));
    if same_set
        itself = knn_indices == repmat((1:n_samples)', 1, n_neighbors);
        vals(itself) = 0;
    end
    vals(knn_fail) = NaN;
    vals = vals';
    vals = vals(:);


function  c = fchcode(b, conn, dir)
%FCHCODE Computes the Freeman chain code of a boundary.
%   C = FCHCODE(B) computes the 8-connected Freeman chain code of a
%   set of 2-D coordinate pairs contained in B, an np-by-2 array. C
%   is a structure with the following fields:
%
%     c.fcc    = Freeman chain code (1-by-np)
%     c.diff   = First difference of code c.fcc (1-by-np)
%     c.mm     = Integer of minimum magnitude from c.fcc (1-by-np)
%     c.diffmm = First difference of code c.mm (1-by-np)
%     c.x0y0   = Coordinates where the code starts (1-by-2)
%
%   C = FCHCODE(B, CONN) produces the same outputs as above, but
%   with the code connectivity specified in CONN. CONN can be 8 for
%   an 8-connected chain code, or CONN can be 4 for a 4-connected
%   chain code. Specifying CONN=4 is valid only if the input
%   sequence, B, contains transitions with values 0, 2, 4, and 6,
%   exclusively.
%
%   C = FHCODE(B, CONN, DIR) produces the same outputs as above, but,
%   in addition, the desired code direction is specified. Values for
%   DIR can be:
%
%     'same'      Same as the order of the sequence of points in b.
%                 This is the default.
%
%     'reverse'   Outputs the code in the direction opposite to the
%                 direction of the points in B.  The starting point
%                 for each DIR is the same.
%
%   The elements of B are assumed to correspond to a 1-pixel-thick,
%   fully-connected, closed boundary. B cannot contain duplicate
%   coordinate pairs, except in the first and last positions, which
%   is a common feature of boundary tracing programs.
%
%   FREEMAN CHAIN CODE REPRESENTATION
%   The table on the left shows the 8-connected Freeman chain codes
%   corresponding to allowed deltax, deltay pairs. An 8-chain is
%   converted to a 4-chain if (1) if conn = 4; and (2) only
%   transitions 0, 2, 4, and 6 occur in the 8-code.  Note that
%   dividing 0, 2, 4, and 6 by 2 produce the 4-code.
%
%       -----------------------  ----------------
%       deltax | deltay | 8-code  corresp 4-code
%       -----------------------  ----------------
%         0        1       0            0
%        -1        1       1
%        -1        0       2            1
%        -1       -1       3
%         0       -1       4            2
%         1       -1       5
%         1        0       6            3
%         1        1       7
%       -----------------------  ----------------
%
%   The formula z = 4*(deltax + 2) + (deltay + 2) gives the following
%   sequence corresponding to rows 1-8 in the preceding table: z =
%   11,7,6,5,9,13,14,15. These values can be used as indices into the
%   table, improving the speed of computing the chain code. The
%   preceding formula is not unique, but it is based on the smallest
%   integers (4 and 2) that are powers of 2.

%   Copyright 2002-2004 R. C. Gonzalez, R. E. Woods, & S. L. Eddins
%   Digital Image Processing Using MATLAB, Prentice-Hall, 2004
%   $Revision: 1.6 $  $Date: 2003/11/21 14:34:49 $

% Preliminaries.
if nargin == 1
   dir = 'same';
   conn = 8;
elseif nargin == 2
   dir = 'same';
elseif nargin == 3
   % Nothing to do here.
else
   error('Incorrect number of inputs.')
end
[np, nc] = size(b);
if np < nc
   error('B must be of size np-by-2.');
end

% Some boundary tracing programs, such as boundaries.m, output a
% sequence in which the coordinates of the first and last points are
% the same. If this is the case, eliminate the last point.
if isequal(b(1, :), b(np, :))
   np = np - 1;
   b = b(1:np, :);
end

% Build the code table using the single indices from the formula
% for z given above:
C(11)=0; C(7)=1; C(6)=2; C(5)=3; C(9)=4;
C(13)=5; C(14)=6; C(15)=7;

% End of Preliminaries.

% Begin processing.
x0 = b(1, 1);
y0 = b(1, 2);
c.x0y0 = [x0, y0];

% Make sure the coordinates are organized sequentially:
% Get the deltax and deltay between successive points in b. The
% last row of a is the first row of b.
a = circshift(b, [-1, 0]);

% DEL = a - b is an nr-by-2 matrix in which the rows contain the
% deltax and deltay between successive points in b. The two
% components in the kth row of matrix DEL are deltax and deltay
% between point (xk, yk) and (xk+1, yk+1).  The last row of DEL
% contains the deltax and deltay between (xnr, ynr) and (x1, y1),
% (i.e., between the last and first points in b).
DEL = a - b;

% If the abs value of either (or both) components of a pair
% (deltax, deltay) is greater than 1, then by definition the curve
% is broken (or the points are out of order), and the program
% terminates.
if any(abs(DEL(:, 1)) > 1) | any(abs(DEL(:, 2)) > 1);
   error('The input curve is broken or points are out of order.')
end

% Create a single index vector using the formula described above.
z = 4*(DEL(:, 1) + 2) + (DEL(:, 2) + 2);

% Use the index to map into the table. The following are
% the Freeman 8-chain codes, organized in a 1-by-np array.
fcc = C(z);

% Check if direction of code sequence needs to be reversed.
if strcmp(dir, 'reverse')
   fcc = coderev(fcc); % See below for function coderev.
end

% If 4-connectivity is specified, check that all components
% of fcc are 0, 2, 4, or 6.
if conn == 4
   val = find(fcc == 1 | fcc == 3 | fcc == 5 | fcc ==7 );
   if isempty(val)
      fcc = fcc./2;
   else
      warning('The specified 4-connected code cannot be satisfied.')
   end
end

% Freeman chain code for structure output.
c.fcc = fcc;

% Obtain the first difference of fcc.
c.diff = codediff(fcc,conn); % See below for function codediff.

% Obtain code of the integer of minimum magnitude.
c.mm = minmag(fcc); % See below for function minmag.

% Obtain the first difference of fcc
c.diffmm = codediff(c.mm, conn);

%-------------------------------------------------------------------%
function cr = coderev(fcc)
%   Traverses the sequence of 8-connected Freeman chain code fcc in
%   the opposite direction, changing the values of each code
%   segment. The starting point is not changed. fcc is a 1-by-np
%   array.

% Flip the array left to right.  This redefines the starting point
% as the last point and reverses the order of "travel" through the
% code.
cr = fliplr(fcc);

% Next, obtain the new code values by traversing the code in the
% opposite direction. (0 becomes 4, 1 becomes 5, ... , 5 becomes 1,
% 6 becomes 2, and 7 becomes 3).
ind1 = find(0 <= cr & cr <= 3);
ind2 = find(4 <= cr & cr <= 7);
cr(ind1) = cr(ind1) + 4;
cr(ind2) = cr(ind2) - 4;

%-------------------------------------------------------------------%
function z = minmag(c)
%MINMAG Finds the integer of minimum magnitude in a chain code.
%   Z = MINMAG(C) finds the integer of minimum magnitude in a given
%   4- or 8-connected Freeman chain code, C. The code is assumed to
%   be a 1-by-np array.

% The integer of minimum magnitude starts with min(c), but there
% may be more than one such value. Find them all,
I = find(c == min(c));
% and shift each one left so that it starts with min(c).
J = 0;
A = zeros(length(I), length(c));
for k = I;
   J = J + 1;
   A(J, :) = circshift(c,[0 -(k-1)]);
end

% Matrix A contains all the possible candidates for the integer of
% minimum magnitude. Starting with the 2nd column, succesively find
% the minima in each column of A. The number of candidates decreases
% as the seach moves to the right on A.  This is reflected in the
% elements of J.  When length(J)=1, one candidate remains.  This is
% the integer of minimum magnitude.
[M, N] = size(A);
J = (1:M)';
for k = 2:N
   D(1:M, 1) = Inf;
   D(J, 1) = A(J, k);
   amin = min(A(J, k));
   J = find(D(:, 1) == amin);
   if length(J)==1
      z = A(J, :);
      return
   end
end

%-------------------------------------------------------------------%
function d = codediff(fcc, conn)
%CODEDIFF Computes the first difference of a chain code.
%   D = CODEDIFF(FCC) computes the first difference of code, FCC. The
%   code FCC is treated as a circular sequence, so the last element
%   of D is the difference between the last and first elements of
%   FCC.  The input code is a 1-by-np vector.
%
%   The first difference is found by counting the number of direction
%   changes (in a counter-clockwise direction) that separate two
%   adjacent elements of the code.

sr = circshift(fcc, [0, -1]); % Shift input left by 1 location.
delta = sr - fcc;
d = delta;
I = find(delta < 0);

type = conn;
switch type
case 4 % Code is 4-connected
   d(I) = d(I) + 4;
case 8 % Code is 8-connected
   d(I) = d(I) + 8;
end


function varargout = drawGrid3d(varargin)
%DRAWGRID3D Draw a 3D grid on the current axis
%
%   drawGrid3d
%   draws a 3D square grid, with origin (0,0,0) and spacing 1 in each
%   direction, with bounds corresponding to the bounds of current axis.
%
%   drawGrid3d(SPACING)
%   where spacing is either a scalar or a [1x3] matrix, specifies the size
%   of the unit cell.
%
%   drawGrid3d(ORIGIN, SPACING)
%   Also specify origin of grid. ORIGIN is a [1x3] array.
%
%   drawGrid3d(..., EDGE)
%   specifies whether function should draw edges touching edges of axis.
%   EDGE is a characheter string, which can be :
%   - 'OPEN' : each line start from one face of window to the opposite
%   face. This results in a 'spiky' grid.
%   - 'CLOSED' (default value) : each line stops at the last visible point
%   of the grid for this line. The result looks like a box (no free spikes
%   around the grid).
%
%   H = drawGrid3d(...);
%   return a vector of handles for each LINE object which was crated.
%

%   ------
%   Author: David Legland
%   e-mail: david.legland@grignon.inra.fr
%   Created: 2005-11-17
%   Copyright 2005 INRA - CEPIA Nantes - MIAJ (Jouy-en-Josas).

%% initialize variables -----

% default values
closed = true;
origin = [0 0 0];
spacing = [1 1 1];

% check if grid is open or not
str = '';
if ~isempty(varargin)
    str = varargin{end};
end
if ischar(str)
    if strncmpi(str, 'open', 4)
        closed = false;
    end
    varargin = varargin(1:end-1);
end

% check origin and grid spacing
if length(varargin)==1
    spacing = varargin{1};
elseif length(varargin)==2
    origin = varargin{1};
    spacing = varargin{2};
end

%% Compute internam data -----

% get axis limits
ax = axis;
x0 = ax(1); x1 = ax(2);
y0 = ax(3); y1 = ax(4);
z0 = ax(5); z1 = ax(6);

% get first and last coordinates of the grid in each direction
dx = spacing(1); dy = spacing(2); dz = spacing(3);
xe = x0 + mod(origin(1) - x0, dx);
xf = x1 - mod(x1 - origin(1), dx);
ye = y0 + mod(origin(2) - y0, dy);
yf = y1 - mod(y1 - origin(2), dy);
ze = z0 + mod(origin(1) - z0, dz);
zf = z1 - mod(z1 - origin(1), dz);

% update first and last coordinate if grid is 'closed'
if closed
    x0 = xe; x1 = xf;
    y0 = ye; y1 = yf;
    z0 = ze; z1 = zf;
end

%% Draw the grid -----

h = [];
%TODO: rewrite code, avoiding loops

% draw lines parallel to x axis
for y = ye:dy:yf
    for z = ze:dz:zf
        h = [h; drawEdge3d([x0 y z x1 y z])]; %#ok<AGROW>
    end
end

% draw lines parallel to y axis
for x = xe:dx:xf
    for z = ze:dz:zf
        h = [h; drawEdge3d([x y0 z x y1 z])]; %#ok<AGROW>
    end
end

% draw lines parallel to z axis
for x = xe:dx:xf
    for y = ye:dy:yf
        h = [h; drawEdge3d([x y z0 x y z1])]; %#ok<AGROW>
    end
end

%% Check output arguments -----

if nargout>0
    varargout{1} = h;
end


function [Acc,p] = largest_component(A,sym)
% LARGEST_COMPONENT Return the largest connected component of A
%
% Acc = largest_component(A) returns the largest connected component
% of the graph A.  If A is directed, this returns the largest
% strongly connected component.
%
% Acc = largest_component(A,1) returns the largest connected piece of
% a directed graph where connectivity is undirected.  Algorithmically,
% this takes A, drops the directions, then components the largest component
% and returns just this piece of the original _directed_ network.  So the
% output Acc is directed in this case.
%
% [Acc,p] = largest_component(A,...) also returns a logical vector
% indicating which vertices in A were chosen.
%
% See also SCOMPONENTS
%
% Example:
%   load_gaimc_graph('dfs_example')
%   [Acc p] = largest_component(A); % compute the largest component
%   xy2 = xy(p,:); labels2 = labels(p); % get component metadata
%   % draw original graph
%   subplot(1,2,1); graph_draw(A,xy,'labels',labels); title('Original');
%   % draw component
%   subplot(1,2,2); graph_draw(Acc,xy2,'labels',labels2); title('Component');

% David F. Gleich
% Copyright, Stanford University, 2008-2009

% History
% 2009-04-29: Initial coding

if ~exist('sym','var') || isempty(sym), sym=0; end

if sym
    As = A|A';
    [ci sizes] = scomponents(As);
else
    [ci sizes] = scomponents(A);
end
[csize cind] = max(sizes);
p = ci==cind;
Acc = A(p,p);


function fem = laplace_smooth(fem_struct,nodenums,iter);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% LAPLACE_SMOOTH performs a Laplacian smoothing, aka "springing", on the
%  mesh. This is achieved by moving each non-boundary point to the average
%  location of the x,y (,z) coordinates of the nodes connected to the given
%  point. The process is repeated according to the 'iter' input parameter.
%  The smoothing may also be performed on a limited portion of the mesh
%  using the 'nodenums' input parameter.
%
% Usage -- fem = laplace_smooth(fem_struct,nodenums,iter);
%                ** nodenums & iter are optional **
%
% Variables
%  fem = updated finite element mesh
%  fem_struct = original finite element mesh
%  nodenums = node numbers of the nodes to be smoothed
%              (default is all nodes)
%  iter = number of iterations of smoothing to perform
%              (default is 5 iterations)
%
% Filename: laplace_smooth.m
% Created by: Ben Holladay
% Date: May 8, 2007
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Parse input parameters. Throw exceptions for mal-formed input.
%
% Desired input variables: fem_struct, (nodenums), (iter)
%
% Defaults and valid flags
iterDefault = 5;
% nodenumsDefault = all non-boundary nodes
%
% Switch on number of input parameters.
switch nargin
   case 0
      error('Not enough input parameters: laplace_smooth() must have a fem_struct.');
   case 1
      iter = iterDefault;
      nodenums = 1:length(fem_struct.x);
   case 2
      iter = iterDefault;
      test = (floor(nodenums) == nodenums) & (nodenums > 0) & ...
         (nodenums <= length(fem_struct.x));
      if ~all(test)
         error('Input parameter ''nodenums'' must be a vector of valid node numbers.');
      end
   case 3
      iterValid = (floor(iter) == iter) && (iter > 0);
      if ~((floor(iter) == iter) && (iter > 0))
         error('Input parameter ''iter'' must be a positive integer');
      end
      test = (floor(nodenums) == nodenums) & (nodenums > 0) & ...
         (nodenums <= length(fem_struct.x));
      if ~all(test)
         error('Input parameter ''nodenums'' must be a vector of valid node numbers.');
      end
   otherwise
      error('Too many input parameters.');
end

% Set flag if their is no z variable.
try
   z = fem_struct.z;
   zflag = true;
catch
   zflag = false;
end

% Initialize basic fem_struct variables.
enodes = fem_struct.e;
x = fem_struct.x;
y = fem_struct.y;
bnd = unique(fem_struct.bnd);
np = size(x,1);

% Nodes to be moved.
moveable = setdiff(1:length(x),bnd);
move = intersect(moveable,nodenums);
conflict = setdiff(nodenums,moveable);
if ~isempty(conflict)
   display('Boundary nodes will not be moved.');
end

% Spring the number of times given by iter.
for k = 1:iter
   % Create sparse matrix of neighbor points x,y,&z coordinates.
   tempx = sparse(enodes,enodes(:,[2,3,1]),x(enodes),np,np);
   tempy = sparse(enodes,enodes(:,[2,3,1]),y(enodes),np,np);
   if zflag
      tempz = sparse(enodes,enodes(:,[2,3,1]),z(enodes),np,np);
   end

   % Calculate the number of neighbors.
   numnghb = sparse(enodes,enodes(:,[2,3,1]),1,np,np);
   numnghb = sum(numnghb);

   % Find the new location.
   tempx = sum(tempx) ./ numnghb;
   tempy = sum(tempy) ./ numnghb;

   % Move nodes.
   x(move) = tempx(move);
   y(move) = tempy(move);

   % Find new location move z coordinate.
   if zflag
      tempz = sum(tempz) ./ numnghb;
      z(move) = tempz(move);
   end
end

% Create output fem_struct.
fem = fem_struct;
fem.x = x;
fem.y = y;
if zflag
   fem.z = z;
end
if is_valid_struct(fem_struct)
    fem = el_areas(fem);
end

return


function [ class, clsize, critvl, ntrans, ifault ] = trnsfr ( varval, class, ...
  clsize, in, ik, iv, critvl )

%*****************************************************************************80
%
%% TRNSFR transfers objects between classes to improve a criterion.
%
%  Discussion:
%
%    This routine is given a classification of objects, including the
%    number of objects in each class, and the current value of some criterion
%    which is desired to be minimized.
%
%    The routine calculates the change in criterion for all possible transfers
%    of any object from its current class to a different class.  Each transfer
%    that would result in a lowering of the criterion is executed, and the
%    related quantities are updated.
%
%    When no more advantageous transfers can be found, the routine returns.
%
%    The routine relies on a user-supplied routine, CRTRAN, to report the
%    expected change in the criterion for a given transfer, and to carry
%    out that transfer if requested.
%
%    The variables CLASS and CRITVL have been added to the argument list
%    of CRTRAN.
%
%    Also, the order of the two classes "L" and "M" was interchanged in
%    the call to CRTRAN.  The original order was counterintuitive.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 February 2008
%
%  Author:
%
%    Original FORTRAN77 version by Banfield, Bassill.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Colin Banfield, LC Bassill,
%    Algorithm AS 113:
%    A transfer for non-hierarchichal classification,
%    Applied Statistics,
%    Volume 26, Number 2, 1977, pages 206-210.
%
%  Parameters:
%
%    Input, real VARVAL(IN,IV), the data values.  There are IN
%    objects, each having spatial dimension IV.
%
%    Input, integer CLASS(IN), the initial classification of
%    each object.
%
%    Input, integer CLSIZE(IK), the initial number of objects in
%    each class.
%
%    Input, integer IN, the number of objects.
%
%    Input, integer IK, the number of classes.
%
%    Input, integer IV, the number of spatial dimensions, or
%    variates, of the objects.
%
%    Input, real CRITVL, the initial value of the criterion.
%
%    Output, integer CLASS(IN), the classification of
%    each object.
%
%    Output, integer CLSIZE(IK), the number of objects in
%    each class.
%
%    Output, real CRITVL, the current value of the criterion.
%
%    Output, integer NTRANS, the number of transfers executed.
%
%    Output, integer IFAULT, error indicator.
%    0, no error detected.
%    1, the number of classes was less than 2.
%    2, the number of objects was less than the number of classes.
%
  eps = 1.0E-38;
  ntrans = 0;

  if ( ik <= 1 )
    ifault = 1;
    return
  end

  if ( in <= ik )
    ifault = 2;
    return
  end

  ifault = 0;
  i = 0;
  icount = 0;

  while ( 1 )

    i = i + 1;

    if ( in <= icount )
      break;
    end

    if ( in < i )
      i = 0;
      icount = 0;
      continue
    end

    m = class(i);
    if ( clsize(m) <= 1 )
      icount = icount + 1;
      continue
    end

    inco = - eps;
    lo = m;
%
%  Test the transfer of object I from class M to class L.
%
    for l = 1 : ik

      if ( l ~= m )

        iswitch = 1;
        inc = crtran ( varval, class, clsize, in, ik, iv, critvl, ...
          i, m, l, iswitch );
%
%  Remember the values of L and INC.
%
        if ( inc < inco )
          lo = l;
          inco = inc;
        end

      end

    end

    icount = icount + 1;
%
%  Execute the transfer of object I from class M to class LO.
%
    if ( lo ~= m )

      l = lo;
      critvl = critvl + inco;
      icount = 0;

      iswitch = 2;
       crtran ( varval, class, clsize, in, ik, iv, critvl, ...
        i, m, l, iswitch );

      ntrans = ntrans + 1;
      class(i) = l;
      clsize(l) = clsize(l) + 1;
      clsize(m) = clsize(m) - 1;

    end

  end

  return
end


function [K] = kv1u0(x, xp, hyp, ubarp, vbarp, dt, i)

logsigmau = hyp(1);
logthetau = hyp(2);
logsigmav = hyp(3);
logthetav = hyp(4);

a1 = hyp(5);
a2 = hyp(6);

n_x = size(x,1);
n_xp = size(xp,1);

x = repmat(x,1,n_xp);
xp = repmat(xp',n_x,1);

ubarp = repmat(ubarp',n_x,1);
vbarp = repmat(vbarp',n_x,1);

switch i

case 0

K=dt.*exp(1).^(logsigmav+(-2).*logthetav+(-1/2).*exp(1).^((-1).*logthetav) ...
  .*(x+(-1).*xp).^2).*(a2.*exp(1).^(2.*logthetav).*(ubarp.^2+vbarp.^2)+ ...
  a1.*((-1).*exp(1).^logthetav+(x+(-1).*xp).^2));

case 1 % logsigmau

K=0;

case 2 % logthetau

K=0;

case 3 % logsigmav

K=dt.*exp(1).^(logsigmav+(-2).*logthetav+(-1/2).*exp(1).^((-1).*logthetav) ...
  .*(x+(-1).*xp).^2).*(a2.*exp(1).^(2.*logthetav).*(ubarp.^2+vbarp.^2)+ ...
  a1.*((-1).*exp(1).^logthetav+(x+(-1).*xp).^2));

case 4 % logthetav

K=(1/2).*dt.*exp(1).^(logsigmav+(-3).*logthetav+(-1/2).*exp(1).^((-1).* ...
  logthetav).*(x+(-1).*xp).^2).*(a1.*(2.*exp(1).^(2.*logthetav)+(-5).*exp( ...
  1).^logthetav.*(x+(-1).*xp).^2+(x+(-1).*xp).^4)+a2.*exp(1).^(2.* ...
  logthetav).*(ubarp.^2+vbarp.^2).*(x+(-1).*xp).^2);

case 5 % a1

K=dt.*exp(1).^(logsigmav+(-2).*logthetav+(-1/2).*exp(1).^((-1).*logthetav) ...
  .*(x+(-1).*xp).^2).*((-1).*exp(1).^logthetav+(x+(-1).*xp).^2);

case 6 % a2

K=dt.*exp(1).^(logsigmav+(-1/2).*exp(1).^((-1).*logthetav).*(x+(-1).*xp) ...
  .^2).*(ubarp.^2+vbarp.^2);

otherwise

        K = zeros(n_x, n_xp);
end

if K == 0

    K = zeros(n_x, n_xp);

end

end


function [R, t, X] = andreff(AA,BB)
% Solves the problem AX=XB
% using the formulation of
%
% On-line Hand-Eye Calibration.
% N. Andreff, R. Horaud, B. Espiau
%
% Mili Shah
% July 2014

[~,n] = size(AA); n = n/4;

A = zeros(12*n,12);
b = zeros(12*n,1);
for i = 1:n
    Ra = AA(1:3,4*i-3:4*i-1);
    Rb = BB(1:3,4*i-3:4*i-1);
    ta = AA(1:3,4*i);
    tb = BB(1:3,4*i);
    A(12*i-11:12*i-3,1:9) = eye(9) - kron(Rb,Ra);
    A(12*i-2:12*i,:) = [kron(tb',eye(3)) eye(3)-Ra];
    b(12*i-2:12*i) = ta;
end
x = pinv(A) * b;

X = reshape(x(1:9),3,3)';
X = sign(det(X))/abs(det(X))^(1/3)*X;

[u, ~, v] = svds(X, 3); X = u*v'; if det(X)<0, X = u*diag([1 1 -1])*v'; end
X = [X' x(10:12);[0 0 0 1]];
R = X(1 : 3, 1 : 3);
t = X(1 : 3, 4);

end


function [RHS, RHSx, RHSy] = ...
    convectionTvdRHS2D(u, phi, FL)
% This function uses the upwind scheme to discretize a 1D
% convection term in the form \grad (u \phi) where u is a face vactor
% It also returns the x and y parts of the matrix of coefficient.
%
% SYNOPSIS:
%   [M, RHS, Mx, My, RHSx, RHSy] = ...
%    convectionTvdTerm2D(u, phi, FL)
%
% PARAMETERS:
%
%
% RETURNS:
%
%
% EXAMPLE:
%
% SEE ALSO:
%

% extract data from the mesh structure
Nx = u.domain.dims(1);
Ny = u.domain.dims(2);
G=reshape(1:(Nx+2)*(Ny+2), Nx+2, Ny+2);
DXp = repmat(u.domain.cellsize.x(2:end-1), 1, Ny);
DYp = repmat(u.domain.cellsize.y(2:end-1)', Nx, 1);
dx=repmat(0.5*(u.domain.cellsize.x(1:end-1)+u.domain.cellsize.x(2:end)), 1, Ny);
dy=repmat(0.5*(u.domain.cellsize.y(1:end-1)+u.domain.cellsize.y(2:end))', Nx, 1);
psiX_p = zeros(Nx+1,Ny);
psiX_m = zeros(Nx+1,Ny);
psiY_p = zeros(Nx,Ny+1);
psiY_m = zeros(Nx,Ny+1);

% define the vectors to stores the sparse matrix data
mnx = Nx*Ny;	mny = Nx*Ny;

% extract the velocity data
% note: size(ux) = [1:m+1, 1:n] and size(uy) = [1:m, 1:n+1]
ux = u.xvalue;
uy = u.yvalue;

% calculate the upstream to downstream gradient ratios for u>0 (+ ratio)
% x direction
dphiX_p = (phi.value(2:Nx+2, 2:Ny+1)-phi.value(1:Nx+1, 2:Ny+1))./dx;
rX_p = dphiX_p(1:end-1,:)./fsign(dphiX_p(2:end,:));
psiX_p(2:Nx+1,:) = 0.5*FL(rX_p).*(phi.value(3:Nx+2,2:Ny+1)-phi.value(2:Nx+1, 2:Ny+1));
psiX_p(1, :) = 0; % left boundary will be handled in the main matrix
% y direction
dphiY_p = (phi.value(2:Nx+1, 2:Ny+2)-phi.value(2:Nx+1, 1:Ny+1))./dy;
rY_p = dphiY_p(:,1:end-1)./fsign(dphiY_p(:,2:end));
psiY_p(:,2:Ny+1) = 0.5*FL(rY_p).*(phi.value(2:Nx+1,3:Ny+2)-phi.value(2:Nx+1, 2:Ny+1));
psiY_p(:,1) = 0; % Bottom boundary will be handled in the main matrix

% calculate the upstream to downstream gradient ratios for u<0 (- ratio)
% x direction
rX_m = dphiX_p(2:end,:)./fsign(dphiX_p(1:end-1,:));
psiX_m(1:Nx,:) = 0.5*FL(rX_m).*(phi.value(1:Nx, 2:Ny+1)-phi.value(2:Nx+1, 2:Ny+1));
psiX_m(Nx+1,:) = 0; % right boundary
% y direction
rY_m = dphiY_p(:,2:end)./fsign(dphiY_p(:,1:end-1));
psiY_m(:,1:Ny) = 0.5*FL(rY_m).*(phi.value(2:Nx+1, 1:Ny)-phi.value(2:Nx+1, 2:Ny+1));
psiY_m(:, Ny+1) = 0; % top boundary will be handled in the main matrix

% reassign the east, west, north, and south velocity vectors for the
% code readability
ue = ux(2:Nx+1,:);		uw = ux(1:Nx,:);
vn = uy(:,2:Ny+1);       vs = uy(:,1:Ny);

% find the velocity direction for the upwind scheme
ue_min = min(ue,0);	ue_max = max(ue,0);
uw_min = min(uw,0);	uw_max = max(uw,0);
vn_min = min(vn,0);	vn_max = max(vn,0);
vs_min = min(vs,0);	vs_max = max(vs,0);

% build the sparse matrix based on the numbering system
rowx_index = reshape(G(2:Nx+1,2:Ny+1),mnx,1); % main diagonal x
rowy_index = reshape(G(2:Nx+1,2:Ny+1),mny,1); % main diagonal y

% calculate the TVD correction term
div_x = -(1./DXp).*((ue_max.*psiX_p(2:Nx+1,:)+ue_min.*psiX_m(2:Nx+1,:))- ...
              (uw_max.*psiX_p(1:Nx,:)+uw_min.*psiX_m(1:Nx,:)));
div_y = -(1./DYp).*((vn_max.*psiY_p(:,2:Ny+1)+vn_min.*psiY_m(:,2:Ny+1))- ...
              (vs_max.*psiY_p(:,1:Ny)+vs_min.*psiY_m(:,1:Ny)));
% define the RHS Vector
RHS = zeros((Nx+2)*(Ny+2),1);
RHSx = zeros((Nx+2)*(Ny+2),1);
RHSy = zeros((Nx+2)*(Ny+2),1);

% assign the values of the RHS vector
RHS(rowx_index) = reshape(div_x+div_y,Nx*Ny,1);
RHSx(rowx_index) = reshape(div_x,Nx*Ny,1);
RHSy(rowy_index) = reshape(div_y,Nx*Ny,1);

end

function phi_out = fsign(phi_in)
% This function checks the value of phi_in and assigns an eps value to the
% elements that are less than or equal to zero, while keeping the signs of
% the nonzero elements
    phi_out = (abs(phi_in)>=eps).*phi_in+eps*(phi_in==0)+eps*(abs(phi_in)<eps).*sign(phi_in);
end


function obj = windsorize(obj, varargin)
% Windsorize an fMRI data object to madlimit Median Absolute Deviations.
% Default = 5 MADs.
% Works across rows and columns.
% Registers this step in history.
%
% :Usage:
% ::
%
%     obj = windsorize(obj, [madlimit])

% ..
%    Calculate and display descriptives
% ..

madlimit = 5;
if ~isempty(varargin), madlimit = varargin{1}; end

dattmp = obj.dat(:);
med = median(dattmp);
mabsd = mad(dattmp);
climits = [med - madlimit * mabsd med + madlimit * mabsd];
wh_out = obj.dat < climits(1) | obj.dat > climits(2);
nout = sum(wh_out(:));
percout = 100 * nout ./ prod(size(obj.dat));

nout_by_case = sum(wh_out, 2);

sep = sprintf('____________________________________________\n');
fprintf('%sfmri_data structure .dat field (data)\n%s', sep, sep);
fprintf('Median: %3.3f\nMean: %3.3f\n', med, mean(dattmp));
fprintf('MAD: %3.3f\nSTD: %3.3f\n', mabsd, std(dattmp));
fprintf('Max:%3.3f\nMin:%3.3f\n', max(dattmp), min(dattmp));
fprintf('Control limits: %3.3f to %3.3f\n', climits);
fprintf('Outliers: %3.0f values, %3.2f%% of all values\n', nout, percout);
fprintf('Outliers by case (row): Max = %3.0f, Median = %3.0f, Min = %3.0f\n', max(nout_by_case), median(nout_by_case), min(nout_by_case));

datadj = obj.dat;
datadj(obj.dat < climits(1)) = climits(1);
datadj(obj.dat > climits(2)) = climits(2);

obj.dat = datadj;
obj.history{end+1} = sprintf('dat windsorized to %3.1f MADs', madlimit);

end % function


function Out   = RiemannLogMap(P,X)

[U Delta] = eig(P);
G = U*sqrt(Delta);
Y = inv(G)*X*inv(G)';
[V Sigma] = eig(Y);
Out = (G*V)*diag(log(diag(Sigma)))*(G*V)';


function maxcorrel=plotcorrelatioondistance(delta_vec,dir1,dir2, name1, name2, marker, col, savethis)
% maxcorrel=plotcorrelatioondistance(delta_vec,res_dir, res_name, marker,
% color savethis)

method='average';
ii=1;
for delta = delta_vec

    for iteration =1:10;
        res_dir = [dir1 num2str(delta*100) dir2];
        res_name = [name1 num2str(iteration) name2];
        load ([res_dir '/' res_name]);
%         load ([peval.data_path peval.data_dir '/' peval.data_name]); %data
        [Wxk,Hkt,centers,Vxkpix]=reshapeGaP(res.hvec,res.cxcy,peval);
        Vxtpixbg=reshape(Wxk*Hkt,peval.nx,peval.ny,peval.nt)+peval.bg;
        resid=(Vxtpixbg-res.dpixc);
        resid_norm=resid./sqrt(Vxtpixbg);
        %         [Z,H,T,perm] = dendrogram_subtreepixels(resid_norm,'average', p , centers(:,1)+1, centers(:,2)+1, savethis)
        data=resid_norm;
        sized = size(data);
        dveccr= reshape(data,sized(1)*sized(2), sized(3));
        ccd = (corrcoef(dveccr'));
%         ccds=squareform(1-ccd);
        e=eye(size(ccd));
        ccdflip=e-(ccd-e);
        ccds=squareform(1-ccdflip);

        Z = linkage(ccds,method);

        if savethis
            save ([res_dir '/corrcoefdist.mat'], 'Z')
        end
        maxcorrel(ii,iteration)=1-min(Z(:,3));
    end
    ii=ii+1;
end

% style='o--r';
% marker='o';
% col='r';
style = [marker col '--'];
figure(1)
hold on
deltanm=delta_vec*106;
m=mean(maxcorrel,2);
s=std(maxcorrel,[],2);
mincorr=min(maxcorrel,[],2);
errorbar(deltanm, m,s,style)
plot(deltanm,mincorr,['-v' col], 'linewidth',2)
xlabel('Separation of sources [nm]')
ylabel('Maximum correlation in residuals')
grid on

figure(2)
hold on
m=mean(maxcorrel,2);
s=std(maxcorrel,[],2);
errorbar(delta_vec, m,s,style)
xlabel('Separation of sources [pix]')
plot(delta_vec,mincorr,['-v' col], 'linewidth',2)
ylabel('Maximum correlation in residuals')
grid on


function [ x_est, x_cov ] = measurement_update(mem_x_pre, mem_cov_pre, meas, ...
    f_func_g, f_jacobian, f_hessian, meas_noise_cov, multi_noise_cov)

% MEASUREMENT_UPDATE: estimate state using second order extended kalman filter (SOEKF)
% Input:
%       mem_x_pre:      previous estimate, [m1, m2, alpha, l1, l2, (velo1, velo2)]
%                       5x1 for 'static', 7x1 for 'NCV'
%       mem_cov_pre:    previous covariance matrix
%                       5x5 for 'static', 7x7 for 'NCV'
%       meas:           measurement, 2x1
%       f_func_g:         quadratic function handle, output of GET_JACOBIAN_HESSIAN function
%       f_jacobian:       handle of Jacobian of func_g, output of GET_JACOBIAN_HESSIAN function
%       f_hessian:        handle of Hessians of func_g, output of GET_JACOBIAN_HESSIAN function
%       meas_noise_cov: covariance of measurement noise, 2x2
%       multi_noise_cov: covariance of multiplicative noise, 2x2, diag(h1_var, h2_var)
% Output:
%       x_est: estimated state
%       x_cov: covariance of state estimate

dim_state = numel(mem_x_pre);
nr_param = 5; % 5 paramtets: m1, m2, alpha, l1, l2

%% shift center to improve robustness
shifted = mem_x_pre(1:2);
meas = meas - shifted;
mem_x_pre_shifted = mem_x_pre;
mem_x_pre_shifted(1:2)=[0;0];

%% augment state and its covariance
X_est = [mem_x_pre_shifted;0;0;0;0];
X_cov = blkdiag(mem_cov_pre, multi_noise_cov, meas_noise_cov);

%% construct pseudo-measurement
z = [meas(1);meas(2);meas(1)^2;meas(2)^2; meas(1)*meas(2)];

%% Substitute quadratic function, Jacobian and Hessian matrices using current estimate
val_subs = [mem_x_pre_shifted(1:nr_param);0;0;0;0]; % velocity does not appear in measurement equation

subs_func_g = f_func_g(val_subs');
subs_jacobian = f_jacobian(val_subs');
subs_hessian = f_hessian(val_subs');

%% calculate variance of pseudo-measurement
cov_zz = zeros(nr_param, nr_param);
for i = 1:nr_param
    for j = 1:nr_param
        cov_zz(i, j) = subs_jacobian(i, :)*X_cov*subs_jacobian(j, :)'+ ...
            (1/2)*trace(subs_hessian(:, :, i)*X_cov*subs_hessian(:, :, j)*X_cov);

    end
end

%% calculate mean of pseudo-measurement
mean_z = zeros(1, nr_param);
for i = 1:nr_param
    mean_z(i) = subs_func_g(i) + (1/2)*trace(subs_hessian(:, :, i)*X_cov);
end

%% calculate cross-variance of pseudo-measurement with augmented estimate
cov_Xz = X_cov*subs_jacobian';

%% Kalman filter update
X_est = double(X_est + cov_Xz*cov_zz^-1 *( z - mean_z'));
X_cov = double(X_cov - cov_Xz*cov_zz^-1*cov_Xz');

X_est(1:2) = X_est(1:2) + shifted;

%% Truncate augmented estimate and covariance
        X_cov = (X_cov+X_cov')/2; % enforce covariance matrix symmetric
        x_est = X_est(1:dim_state);
        x_cov = X_cov(1:dim_state, 1:dim_state);
end


% [INPUT]
% dep = A vector of floats of length t representing the dependent variable.
% indep_s = A float t-by-cs matrix (-Inf,Inf) representing the switching independent variables, without intercept because it is internally handled by the model (optional, default=[]).
% indep_ns = A float t-by-cns matrix (-Inf,Inf) representing the non-switching independent variables (optional, default=[]).
% k = An integer [2,4] representing the number of states of the model (optional, default=2).
% vs = A boolean that indicates whether the variance is allowed to switch (optional, default=true).
% finit = A function handle representing a hook for generating a customized minimization problem (optional, by default a standard minimization problem is generated).
%   The function handle must accept the following input arguments (in the same order):
%     - x0: A column vector of floats of length cx representing the initial parameters.
%     - dep, indep_s, indep_s, k, vs, tmm: See the description above.
%     - p0 = A float k-by-k matrix [0,1] representing the initial stochastic row-wise transition matrix.
%     - options = A structure representing the optimization options structure.
%   The function handle must return the following output arguments (in the same order):
%     - x0 = A column vector of floats of length cx = (vs ? k : 1) + (cs * k) + cns representing the initial parameters for variance and independent variables (mandatory).
%     - ai = A float ci-by-cx matrix (-Inf,Inf) representing the "A" element of linear inequality constraints (optional, defaultable to []).
%     - bi = A column vector of floats of length ci representing the "b" element of linear inequality constraints (optional, defaultable to []).
%     - ae = A float ce-by-cx matrix (-Inf,Inf) representing the "Aeq" element of linear equality constraints (optional, defaultable to []).
%     - ce = A column vector of floats of length ce representing the "beq" element of linear equality constraints (optional, defaultable to []).
%     - lb = A column vector of floats of length cx representing the lower bounds (mandatory).
%     - ub = A column vector of floats of length cx representing the upper bounds (mandatory).
% tmm = A float k-by-k matrix [0,1] representing the mask of the stochastic row-wise transition matrix, in which NaNs represent the elements to be computed (optional, by default all the elements are computed).
% fnlcon = A function handle representing a hook for applying non-linear constraints (optional, by default no non-linear constraints are applied).
%   The function handle must accept the following input arguments (in the same order):
%     - x: A column vector of floats representing the current parameters.
%     - dep, indep_s, indep_s, k, vs, tmm: See the description above.
%     - options = A structure representing the optimization options structure.
%   The function handle must return the following output arguments (in the same order):
%     - ci = A column vector of floats representing the "C" element of non-linear constraints (optional, defaultable to []).
%     - ce = A column vector of floats representing the "Ceq" element of non-linear constraints (optional, defaultable to []).
%
% [OUTPUT]
% indep_s_params = A cs-by-1 cell array of row vectors of floats containing the parameters of switching independent variables.
% indep_ns_params = A row vector of floats of length cns representing the parameters of non-switching independent variables.
% s2_params = A row vector of floats of length k representing the variance parameters.
% p = A float k-by-k matrix [0,1] representing the stochastic row-wise transition matrix.
% sprob = A float t-by-k matrix [0,1] representing the smoothed probabilities of each state.
% dur = A row vector of floats of length 4 representing the duration of each state.
% cmu = A column vector of floats of length t representing the conditional means.
% cs2 = A column vector of floats of length t representing the conditional variances.
% e = A column vector of floats of length t representing the standardized residuals.

function [indep_s_params,indep_ns_params,s2_params,p,sprob,dur,cmu,cs2,e] = regime_switching(varargin)

    persistent ip;

    if (isempty(ip))
        ip = inputParser();
        ip.addRequired('dep',@(x)validateattributes(x,{'double'},{'real' 'finite' 'column' 'nonempty'}));
        ip.addOptional('indep_s',[],@(x)validateattributes(x,{'double'},{'real' 'finite' '2d'}));
        ip.addOptional('indep_ns',[],@(x)validateattributes(x,{'double'},{'real' 'finite' '2d'}));
        ip.addOptional('k',2,@(x)validateattributes(x,{'double'},{'real' 'finite' 'integer' '>=' 2 '<=' 4 'scalar'}));
        ip.addOptional('vs',true,@(x)validateattributes(x,{'logical'},{'scalar'}));
        ip.addOptional('finit',[],@(x)validateattributes(x,{'double' 'function_handle'},{}));
        ip.addOptional('tmm',[],@(x)validateattributes(x,{'double'},{'real' '2d'}));
        ip.addOptional('fnlcon',[],@(x)validateattributes(x,{'double' 'function_handle'},{}));
    end

    ip.parse(varargin{:});

    ipr = ip.Results;
    vs = ipr.vs;
    [dep,indep_s,indep_ns,k,finit,tmm,p0,fnlcon] = validate_input(ipr.dep,ipr.indep_s,ipr.indep_ns,ipr.k,ipr.finit,ipr.tmm,ipr.fnlcon);

    nargoutchk(6,9);

    [indep_s_params,indep_ns_params,s2_params,p,sprob,dur,cmu,cs2,e] = regime_switching_internal(dep,indep_s,indep_ns,k,vs,finit,tmm,p0,fnlcon);

end

function [indep_s_params,indep_ns_params,s2_params,p,sprob,dur,cmu,cs2,e] = regime_switching_internal(dep,indep_s,indep_ns,k,vs,finit,tmm,p0,fnlcon)

    persistent options;

    if (isempty(options))
        options = optimset(optimset(@fmincon),'Algorithm','sqp','Diagnostics','off','Display','off','LargeScale','off','MaxSQPIter',1000,'TolFun',1e-6);
    end

    t = numel(dep);

    [p_params,p_lb,p_ub,p_ae,p_be] = parametrize_p(k,tmm,p0);
    p_params_count = numel(p_params);

    if (isempty(finit))
        x0 = [];
        [ai,bi] = deal([]);
        [ae,be] = deal([]);
        [lb,ub] = deal([]);

        [s2_params,s2_lb,s2_ub] = parametrize_s2(dep,k,vs,1.5,0.75);
        s2_params_count = numel(s2_params);
        x0 = [x0; s2_params];
        lb = [lb; s2_lb];
        ub = [ub; s2_ub];

        [indep_s_params,indep_s_lb,indep_s_ub] = parametrize_indep_s(dep,indep_s,k);
        indep_s_params_count = numel(indep_s_params);
        x0 = [x0; indep_s_params];
        lb = [lb; indep_s_lb];
        ub = [ub; indep_s_ub];

        [indep_ns_params,indep_ns_lb,indep_ns_ub] = parametrize_indep_ns(dep,indep_ns);
        indep_ns_params_count = numel(indep_ns_params);
        x0 = [x0; indep_ns_params];
        lb = [lb; indep_ns_lb];
        ub = [ub; indep_ns_ub];

        params_count = indep_s_params_count + indep_ns_params_count + s2_params_count;
    else
        [x0,ai,bi,ae,be,lb,ub] = finit(dep,indep_s,indep_ns,k,vs,tmm,p0,options);

        if (vs)
            s2_params_count = k;
        else
            s2_params_count = 1;
        end

        indep_s_params_count = size(indep_s,2) * k;
        indep_ns_params_count = size(indep_ns,2);

        params_count = s2_params_count + indep_s_params_count + indep_ns_params_count;

        if (~isa(x0,'double') || ~ismatrix(x0) || (size(x0,2) ~= 1) || (size(x0,1) ~= params_count))
            error(['The function ''finit'' generated an invalid value for ''x0'': it must be a float column vector of length ' num2str(params_count) '.']);
        end

        if (~isempty(ai) || ~isempty(bi))
            if (~isa(ai,'double') || ~ismatrix(ai))
                error(['The function ''finit'' generated an invalid value for ''ai'': it must be a float 2d matrix with no zero-valued dimensions and the number of columns equal to ' num2str(params_count) '.']);
            end

            [ai_r,ai_c] = size(ai);

            if ((ai_r == 0) || (ai_c ~= params_count))
                error(['The function ''finit'' generated an invalid value for ''ai'': it must be a float 2d matrix with no zero-valued dimensions and the number of columns equal to ' num2str(params_count) '.']);
            end

            if (~isa(bi,'double') || ~ismatrix(bi))
                error(['The function ''finit'' generated an invalid value for ''bi'': it must be a a column vector of length ' num2str(ai_r) '.']);
            end

            [bi_r,bi_c] = size(bi);

            if ((bi_r ~= ai_r) || (bi_c ~= 1))
                error(['The function ''finit'' generated an invalid value for ''bi'': it must be a a column vector of length ' num2str(ai_r) '.']);
            end

            ai = [ai zeros(ai_r,p_params_count)];
        end

        if (~isempty(ae) || ~isempty(be))
            if (~isa(ae,'double') || ~ismatrix(ae))
                error(['The function ''finit'' generated an invalid value for ''ae'': it must be a float 2d matrix with no zero-valued dimensions and the number of columns equal to ' num2str(params_count) '.']);
            end

            [ae_r,ae_c] = size(ae);

            if ((ae_r == 0) || (ae_c ~= params_count))
                error(['The function ''finit'' generated an invalid value for ''ae'': it must be a float 2d matrix with no zero-valued dimensions and the number of columns equal to ' num2str(params_count) '.']);
            end

            if (~isa(be,'double') || ~ismatrix(be))
                error(['The function ''finit'' generated an invalid value for ''be'': it must be a a column vector of length ' num2str(ae_r) '.']);
            end

            [be_r,be_c] = size(be);

            if ((be_r ~= ae_r) || (be_c ~= 1))
                error(['The function ''finit'' generated an invalid value for ''be'': it must be a a column vector of length ' num2str(ae_r) '.']);
            end

            ae = [ae zeros(ae_r,p_params_count)];
        end

        if (~isa(lb,'double') || ~ismatrix(lb) || (size(lb,2) ~= 1) || (size(lb,1) ~= params_count))
            error(['The function ''finit'' generated an invalid value for ''lb'': it must be a float column vector of length ' num2str(params_count) '.']);
        end

        if (~isa(ub,'double') || ~ismatrix(ub) || (size(ub,2) ~= 1) || (size(ub,1) ~= params_count))
            error(['The function ''finit'' generated an invalid value for ''ub'': it must be a float column vector of length ' num2str(params_count) '.']);
        end
    end

    x0 = [x0; p_params];
    ae = [ae; zeros(k,params_count) p_ae];
    be = [be; p_be];
    lb = [lb; p_lb];
    ub = [ub; p_ub];

    if (isempty(fnlcon))
        params = fmincon(@(x)likelihood(x,dep,indep_s,indep_ns,k,vs,tmm),x0,ai,bi,ae,be,lb,ub,[],options);
    else
        params = fmincon(@(x)likelihood(x,dep,indep_s,indep_ns,k,vs,tmm),x0,ai,bi,ae,be,lb,ub,@(x)fnlcon(x,dep,indep_s,indep_ns,k,vs,tmm,options),options);
    end

    [~,mu,g] = likelihood(params,dep,indep_s,indep_ns,k,vs,tmm);

    if (vs)
        s2_params = params(1:k).';
        o = k + 1;
    else
        s2_params = ones(1,k) .* params(1);
        o = 2;
    end

    indep_s_count = size(indep_s,2);
    indep_s_params = num2cell(reshape(params(o:o+indep_s_params_count-1),k,indep_s_count).',2);
    o = o + indep_s_params_count;

    if (indep_ns_params_count > 0)
        indep_ns_params = params(o:o+indep_ns_params_count-1).';
        o = o + indep_ns_params_count;
    else
        indep_ns_params = [];
    end

    p = tmm;
    p(isnan(tmm)) = params(o:end);

    pt = p.';

    prob = [ones(1,k) .* (1 / k); zeros(t - 1,k)];

    for i = 2:t
        prob(i,:) = pt * g(i-1,:).';
    end

    sprob = [zeros(t - 1,k); g(t,:)];

    for i = t-1:-1:1
        for j = 1:k
            sprob(i,j) = sum((sprob(i+1,:) .* g(i,j) .* pt(:,j).') ./ prob(i+1,:),'omitnan');
        end
    end

    dur = round(1 ./ (1 - diag(p).'),0);

    cmu = sum(mu .* prob,2);
    cs2 = sum(repmat(sqrt(s2_params),t,1) .* prob,2);

    e = dep - cmu;
    e = (e - mean(e)) ./ std(e);

end

function [ll,mu,g] = likelihood(x,dep,indep_s,indep_ns,k,vs,tmm)

    t = numel(dep);

    if (vs)
        c = x(1:k);
        o = k + 1;
    else
        c = ones(k,1) .* x(1);
        o = 2;
    end

    indep_s_count = size(indep_s,2);
    indep_s_params_count = indep_s_count * k;
    indep_s_params = reshape(x(o:o+indep_s_params_count-1),k,indep_s_count).';
    o = o + indep_s_params_count;

    indep_ns_count = size(indep_ns,2);

    if (indep_ns_count == 0)
        indep_ns = zeros(t,1);
        indep_ns_params = 0;
    else
        indep_ns_params = x(o:o+indep_ns_count-1);
        o = o + indep_ns_count;
    end

    p = tmm;
    p(isnan(tmm)) = x(o:end);

    pt = p.';

    mu = zeros(t,k);
    z = zeros(t,k);
    nc = (2 * pi())^0.5;

    for i = 1:k
        c_i = c(i);

        mu_i = (indep_s * indep_s_params(:,i)) + (indep_ns * indep_ns_params);
        e_i = dep - mu_i;

        mu(:,i) = mu_i;
        z(:,i) = (1 / (nc * sqrt(c_i))) .* exp(-0.5 .* sum((e_i / c_i) .* e_i,2));
    end

    w1 = (pt * (ones(k,1) .* (1 / k))) .* z(1,:).';
    f1 = ones(1,k) * w1;

    f = [f1; zeros(t - 1,1)];
    g = [(w1 ./ f1).'; zeros(t - 1,k)];

    for i = 2:t
        wi = (pt * g(i-1,:).') .* z(i,:).';
        fi = ones(1,k) * wi;

        f(i,1) = fi;
        g(i,:) = wi ./ fi;
    end

    ll_v = log(f(2:end));

    if (any(~isfinite(ll_v)))
        ll = Inf;
    else
        ll = -sum(ll_v);
    end

end

function [params,lb,ub] = parametrize_indep_ns(dep,indep_ns)

    indep_ns_count = size(indep_ns,2);

    if (indep_ns_count > 0)
        params = regress(dep,indep_ns);
        lb = -Inf(indep_ns_count,1);
        ub = Inf(indep_ns_count,1);
    else
        params = [];
        lb = [];
        ub = [];
    end

end

function [params,lb,ub] = parametrize_indep_s(dep,indep_s,k)

    indep_s_count = size(indep_s,2);

    b = regress(dep,indep_s);
    b_factor = 1;

    indep_s_params = zeros(indep_s_count,k);

    for i = 1:k
        indep_s_params(:,i) = b * b_factor;
        b_factor = b_factor * -1;
    end

    params = reshape(indep_s_params.',indep_s_count * k,1);
    lb = -Inf(numel(params),1);
    ub = Inf(numel(params),1);

end

function [params,lb,ub,ae,be] = parametrize_p(k,tmm,p0)

    tmm_nans = isnan(tmm);

    if (all(all(tmm_nans)))
        params = p0(:);

        lb = zeros(k^2,1);
        ub = ones(k^2,1) - 1e-4;

        ae = repmat(eye(k),1,k);
        be = ones(k,1);
    else
        filter = ~tmm_nans(:);

        params = p0(:);
        params(filter) = [];

        lb = zeros(numel(params),1);
        ub = ones(numel(params),1);

        ae = repmat(eye(k),1,k);
        ae(:,filter) = [];

        be = ones(k,1) -  sum(p0 .* ~tmm_nans,2);
    end

end

function [params,lb,ub] = parametrize_s2(dep,k,vs,factor,multiplier)

    mm = (dep - mean(dep)).^2;
    s2 = var(dep);

    if (vs)
        params = zeros(k,1);

        for i = 1:k
            params(i) = s2 * multiplier;
            multiplier = multiplier * factor;
        end

        lb = ones(k,1) .* min(mm);
        ub = ones(k,1) .* max(mm);
    else
        params = s2 * multiplier;

        lb = min(mm);
        ub = max(mm);
    end

end

function [dep,indep_s,indep_ns,k,finit,tmm,p0,fnlcon] = validate_input(dep,indep_s,indep_ns,k,finit,tmm,fnlcon)

    dep = dep(:);
    t = numel(dep);

    if (~isempty(indep_s))
        if (size(indep_s,1) ~= t)
            error(['The value of ''indep_s'' is invalid. Expected input to have ' num2str(t) ' rows.']);
        end

        if (all(indep_s(:,1) == 1))
            error('The value of ''indep_s'' is invalid. Expected input to exclude the intercept because it is internally handled by the model.');
        end

        if (any(sum(indep_s == 0,1) == t))
            error('The value of ''indep_s'' is invalid. Expected input to contain no zero-valued vectors.');
        end
    end

    indep_s = [ones(t,1) indep_s];

    if (~isempty(indep_ns) && (size(indep_ns,1) ~= t))
        if (size(indep_ns,1) ~= t)
            error(['The value of ''indep_ns'' is invalid. Expected input to have ' num2str(t) ' rows.']);
        end

        if (any(sum(indep_ns,1) == t) || any(sum(indep_ns == 0,1) == t))
            error('The value of ''indep_ns'' is invalid. Expected input to contain no zero-valued or one-valued vectors.');
        end
    end

    if (~isempty(finit))
        if (~isa(finit,'function_handle') || ~isscalar(finit))
            error('The value of ''finit'' is invalid. Expected input to be a single function handle.');
        end

        if (nargin(finit) ~= 8)
            error('The value of ''finit'' is invalid. Expected input to accept 8 input arguments.');
        end

        if (nargout(finit) ~= 7)
            error('The value of ''finit'' is invalid. Expected input to accept 7 output arguments.');
        end
    end

    if (isempty(tmm))
        tmm = NaN(k,k);
        p0 = repmat(0.1,k,k) + (eye(k) * (1 - (k * 0.1)));
    else
        if (any(size(tmm) ~= k))
            error(['The value of ''tmm'' is invalid. Expected input to be a matrix of size ' num2str(k) 'x' num2str(k) '.']);
        end

        tmm_nans = isnan(tmm);
        p0 = zeros(k,k);

        for i = 1:k
            tmm_i = tmm(i,:);
            tmm_nans_i = tmm_nans(i,:);
            tmm_nans_i_count = sum(tmm_nans_i);

            if (tmm_nans_i_count == k)
                p0(i,i) = (1 - (k * 0.1)) + 0.1;

                tmm_nans_i(i) = 0;
                p0(i,tmm_nans_i) = 0.1;
            else
                if (any((tmm_i < 0) | (tmm_i >= 1)))
                    error(['The value of ''tmm'' is invalid. Expected input to have a valid definition for row ' num2str(i) ': all the constrained elements must have a value in range [0,1).']);
                end

                if (tmm_nans_i_count == 0)
                    if (sum(tmm_i) ~= 1)
                        error(['The value of ''tmm'' is invalid. Expected input to have a valid definition for row ' num2str(i) ': the sum of elements must be equal to 1.']);
                    end
                elseif (tmm_nans_i_count == 1)
                    if (k == 2)
                        error(['The value of ''tmm'' is invalid. Expected input to have a valid definition for row ' num2str(i) ': when the number of states is equal to 2, only fully constrained or fully unconstrained rows are accepted.']);
                    else
                        error(['The value of ''tmm'' is invalid. Expected input to have a valid definition for row ' num2str(i) ': the number of unconstrained elements must be greater than 1.']);
                    end
                else
                    if (sum(tmm_i(~tmm_nans_i)) >= 1)
                        error(['The value of ''tmm'' is invalid. Expected input to have a valid definition for row ' num2str(i) ': the number of constrained elements must less than 1.']);
                    end
                end

                if (~tmm_nans_i(i) && (tmm_i(i) == 0))
                    error(['The value of ''tmm'' is invalid. Expected input to have a valid definition for row ' num2str(i) ': diagonal elements cannot be constrained to a value equal to 0.']);
                end

                p0(i,~tmm_nans_i) = tmm_i(~tmm_nans_i);

                k_i = sum(tmm_nans_i);
                delta = 1 - sum(tmm_i(~tmm_nans_i));

                if (tmm_nans_i(i))
                    pd = 0.7 * delta;
                    pnd = (delta - pd) / (k_i - 1);

                    p0(i,i) = pd;

                    tmm_nans_i(i) = 0;
                    p0(i,tmm_nans_i) = pnd;
                else
                    p0(i,tmm_nans_i) = delta / sum(tmm_nans_i);
                end
            end
        end

        we = (k - 1)^2 + 1;
        q = p0^we;
        is_ergodic = ~any(q(:) < (k * eps()));

        if (~is_ergodic)
            error('The value of ''tmm'' is invalid. Expected input to produce an ergodic stochastic row-wise transition matrix.');
        end
    end

    if (~isempty(fnlcon))
        if (~isa(fnlcon,'function_handle') || ~isscalar(fnlcon))
            error('The value of ''fnlcon'' is invalid. Expected input to be a single function handle.');
        end

        if (nargin(fnlcon) ~= 8)
            error('The value of ''fnlcon'' is invalid. Expected input to accept 8 input arguments.');
        end

        if (nargout(fnlcon) ~= 2)
            error('The value of ''fnlcon'' is invalid. Expected input to accept 2 output arguments.');
        end
    end

end


function K = calWholeStiffnessMatrix(coord, unit_topology_table, materials,cal_type)
% 这个函数计算一维半带宽方法下的整体的劲度矩阵K
% 输入：
%     节点坐标表
%     单元拓扑表
%     材料情况
%       计算模式 cal_type
%            单元应力问题
%            单元应变问题
% 输出：
%     整体刚度矩阵 K
%     K 为cell
%     K{1，1}为转化为一维带状储存的K
%     K{1，2}为主对角元在一维矩阵中位置

%使用openKspeace（）函数计算带状矩阵K所需要的空间
    [K, K_info] = openKspeace(unit_topology_table);
    m           = size(unit_topology_table,1);
    t           = 1;
    for i = 1:m
        element_X = coord(unit_topology_table(i, :)',1);
        element_Y = coord(unit_topology_table(i, :)',2);
        matrixB   = calMatrixB(element_X, element_Y);
        matrixD   = calMatrixD(materials(i,1), materials(i,2), cal_type);
        element_k = calElementStiffnessMatrix(matrixB, matrixD, t,...
                                              calAera(element_X, element_Y));
        position  = zeros(1,6);
        for ii = 1:3
            position(ii*2-1) = unit_topology_table(i,ii)*2 - 1;
            position(ii*2)   = unit_topology_table(i,ii)*2;
        end
        for j = 1:6
            for jj = 1:6
                if position(j) >= position(jj)
                oned_x    = K_info(position(j)) - (position(j)-position(jj));
                K(oned_x) = K(oned_x) + element_k(j,jj);
                end
            end
        end
    end
    temp = K;
    clear K;
    K{1} = temp;
    K{2} = K_info;


%SF_SIMP_P1 Linear Lagrange shape function for simplices (P1).
%
%   [ VBASE, NLDOF, XLDOF, SFUN ] = SF_SIMP_P1( I_EVAL, N_SDIM, N_VERT, I_DOF, XI, AINVJAC, VBASE )
%   Evaluates conforming linear P1 Lagrange shape functions on simplices with
%   values defined in the nodes. XI is Barycentric coordinates.
%
%       Input       Value/[Size]           Description
%       -----------------------------------------------------------------------------------
%       i_eval      scalar:  1             Evaluate function values
%                           >1             Evaluate values of derivatives
%       n_sdim      scalar: 1-3            Number of space dimensions
%       n_vert      scalar: 2-4            Number of vertices per cell
%       i_dof       scalar: 1-n_ldof       Local basis function to evaluate
%       xi          [n_sdim+1]             Local coordinates of evaluation point
%       aInvJac     [n,n_sdim+1*n_sdim]    Inverse of transformation Jacobian
%       vBase       [n]                    Preallocated output vector
%                                                                                .
%       Output      Value/[Size]           Description
%       -----------------------------------------------------------------------------------
%       vBase       [n]                    Evaluated function values
%       nLDof       [4]                    Number of local degrees of freedom on
%                                          vertices, edges, faces, and cell interiors
%       xLDof       [n_sdim,n_ldof]        Local coordinates of local dofs
%       sfun        string                 Function name of called shape function
%
%   See also SFLAG1

% Copyright 2013-2022 Precise Simulation, Ltd.


function [OAoftrain,penalty] = svm_train_linear(traindata,trainlabel,rangeOfpenalty,incrOfpenalty)
% Using the training data and corresponding label to get the best penalty
% with respect to highest accuracy which is the result of 5-fold
% cross-validation(default).
% Using the grid search to look for the pair of parameters:(penalty)
% penalty lies in the range of rangeOfpenalty in the speed of incrOfpenalty

% svmtrain/svmpredict:
%       data:row represents # of samples, column represents # of features
%       label:column vector
% rangeOfpenalty:2-dims row vector
% incrOfpenalty:scaler on the basis of 2

i = 0;
row = size(rangeOfpenalty(1):incrOfpenalty:rangeOfpenalty(2),2);
OAoftrain = cell(1,row);
OAbest = 0;
penalty = 2^(rangeOfpenalty(1));
for iter_c = rangeOfpenalty(1):incrOfpenalty:rangeOfpenalty(2)
    i = i+1;
    cmd = ['-v 5 -t 0 -c ',sprintf('%.16f',2^(iter_c))];
    OAoftrain{i} = svmtrain(trainlabel,traindata,cmd);
    if OAoftrain{i} > OAbest
        penalty = 2^(iter_c);
        OAbest = OAoftrain{i};
    end
end


function A = warmUpExercise()
%WARMUPEXERCISE Example function in octave
%   A = WARMUPEXERCISE() is an example function that returns the 5x5 identity matrix

A = eye(5);
% ============= YOUR CODE HERE ==============
% Instructions: Return the 5x5 identity matrix
%               In octave, we return values by defining which variables
%               represent the return values (at the top of the file)
%               and then set them accordingly.

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

end


function [err, paramsOut] = showErrors(params, gt, figName, verbose)
%% Compute calibration errors given stereo parameters from synthetic calibration.
% Error types:
% 1. General errors
%   a. Reprojection error
%   b. 3D alignment error: RMS of Euclidean distance between reconstructed and
%   ground truth point cloud.
%   c. Rotation error
%   d. Translation error
% 2. Projector intrinsics errors
% 3. Camera intrinsics errors
% See also: Calibration.poseErr, Calibration.alignmentErrorSynthetic

%% License
% ACADEMIC OR NON-PROFIT ORGANIZATION NONCOMMERCIAL RESEARCH USE ONLY
% Copyright (c) 2018 Bingyao Huang
% All rights reserved.

% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:

% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.

% If you publish results obtained using this software, please cite our paper.

% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
% SOFTWARE.

%%

if(nargin < 4)
    verbose = false;
end

% concatenate every plane's gt points for 3D alignment error calculation
gtPtsCamImg = cell2mat(gt.synthData.gtPtsCamImg');
gtPtsPrjImg = cell2mat(gt.synthData.gtPtsPrjImg');
gtPtsWorld =  cell2mat(gt.synthData.gtPtsWorld');

%% unpack parameters
% intrinsics
camK = params.camK;
camKc = params.camKc;
prjK = params.prjK;
prjKc = params.prjKc;

% extrinsics
R = params.R;
T = params.T;

rvec = cv.Rodrigues(R)';
%% Reconstruct gt points using calibrated parameters
% VERY IMPORTANT!!!!, undistort camera image points first
gtCamPtsUndistort = ImgProc.cvUndistortPoints(gtPtsCamImg, camK, camKc);
gtCrjPtsUndistort = ImgProc.cvUndistortPoints(gtPtsPrjImg, prjK, prjKc);

% triangulate
reconPtsWorld = Reconstruct.triangulatePoints(camK, prjK, R, T, gtCamPtsUndistort, gtCrjPtsUndistort);
paramsOut.reconPtsWorld = reconPtsWorld;

% visualize
if(verbose)
    Reconstruct.visualizePts3d(reconPtsWorld, R, T, figName);
end

%% Reprojection error (sum of cam and prj)
camPts2d = cv.projectPoints(reconPtsWorld, [0 0 0], [0 0 0], camK, 'DistCoeffs', camKc);
prjPts2d = cv.projectPoints(reconPtsWorld, rvec, T', prjK, 'DistCoeffs', prjKc);

% compute residual between reprojected pts2d and captured pts2d
camReprojRes = camPts2d - gtPtsCamImg;
prjReprojRes = prjPts2d - gtPtsPrjImg;

stereoReprojRes = [camReprojRes;prjReprojRes];

% root mean square reprojection error
rmsReprojErr = sqrt( mean(sum(stereoReprojRes.^2, 2)) );

%% 3D alignment error
rmsAlignErr = Calibration.alignmentErrorSynthetic(reconPtsWorld, gtPtsWorld, verbose, figName);

%% Parameteres estimation error
% intrinsics error
camKErr = params.camK - gt.camK;
camKcErr = params.camKc - gt.camKc;
prjKErr = params.prjK - gt.prjK;
prjKcErr = params.prjKc - gt.prjKc;

% pose error (R, T)
[rotErr, transErr] = Calibration.poseErr([params.R, params.T], [gt.R, gt.T] );

%% General errors
err.name = figName;

% reconstruction, reprojection and 3d alignment error
% err.rmsReconErr = rmsReconErr;
err.rmsReprojErr = rmsReprojErr;
err.rmsAlignErr = rmsAlignErr;
% err.mEpipolarErr = mEpipolarErr;

% extrinsic err
err.rotErr = rotErr;
err.transErr = transErr;

%% Projector errors
% projection matrix err (absolute percentage value)
err.fxErrPrj = abs(prjKErr(1,1) / gt.prjK(1,1)) * 100;
err.fyErrPrj = abs(prjKErr(2,2) / gt.prjK(2,2)) * 100;

% projection matrix err (absolute value)
err.cxErrPrj = abs(prjKErr(1,3));
err.cyErrPrj = abs(prjKErr(2,3));

% distortion factors err (absolute percentage value)
err.k1ErrPrj = abs(prjKcErr(1) / gt.prjKc(1)) * 100;
err.k2ErrPrj = abs(prjKcErr(2) / gt.prjKc(2)) * 100;
err.p1ErrPrj = abs(prjKcErr(3) / gt.prjKc(3)) * 100;
err.p2ErrPrj = abs(prjKcErr(4) / gt.prjKc(4)) * 100;

%% Camera error

err.fxErrCam = abs(camKErr(1,1) / gt.camK(1,1)) * 100;
err.fyErrCam = abs(camKErr(2,2) / gt.camK(2,2)) * 100;

% projection matrix err (absolute value)
err.cxErrCam = abs(camKErr(1,3));
err.cyErrCam = abs(camKErr(2,3));

% distortion factors err (absolute percentage value)
err.k1ErrCam = abs(camKcErr(1) / gt.camKc(1)) * 100;
err.k2ErrCam = abs(camKcErr(2) / gt.camKc(2)) * 100;
err.p1ErrCam = abs(camKcErr(3) / gt.camKc(3)) * 100;
err.p2ErrCam = abs(camKcErr(4) / gt.camKc(4)) * 100;

end


function points = robotlaser_as_cartesian(rl, maxRange = 15, subsample = false)

numBeams = length(rl.ranges);
maxRange=min(maxRange, rl.maximum_range);
% apply the max range
idx = rl.ranges<maxRange & rl.ranges>0;

if (subsample)
	idx(2:2:end) = 0;
endif

angles = linspace(rl.start_angle, rl.start_angle + numBeams*rl.angular_resolution, numBeams)(idx);
points = [rl.ranges(idx) .* cos(angles); rl.ranges(idx) .* sin(angles); ones(1, length(angles))];
transf = v2t(rl.laser_offset);

% apply the laser offset
points = transf * points;

end


function w = dwt2D(x, J, af)

% discrete 2-D wavelet transform
%
% USAGE:
%   w = dwt2D(x, stages, af)
% INPUT:
%   x - N by M matrix
%       1) M, N are both even
%       2) min(M,N) >= 2^(J-1)*length(af)
%   J - number of stages
%   af - analysis filters
% OUPUT:
%   w - cell array of wavelet coefficients
% EXAMPLE:
%   [af, sf] = farras;
%   x = rand(128,64);
%   J = 3;
%   w = dwt2D(x,J,af);
%   y = idwt2D(w,J,sf);
%   err = x - y;
%   max(max(abs(err)))
%
% WAVELET SOFTWARE AT POLYTECHNIC UNIVERSITY, BROOKLYN, NY
% http://taco.poly.edu/WaveletSoftware/

for k = 1:J
    [x w{k}] = afb2D(x, af, af);
end
w{J+1} = x;


function x = reErr(ref, sig)
%
% psnr -- Compute relative error for images
%
% Usage:
%       x = reErr(ref, sig)
%
% Input:
%       ref         Reference image
%       sig         Modified image
%
% Output:
%       x           reErr value
%
% Authors
%   Paul Rodriguez    prodrig@pucp.edu.pe
%   Brendt Wohlberg   brendt@tmail.lanl.gov
%

[Nrows Ncols depth] = size(ref);

num = mean((ref(:)-sig(:)).^2);
den = mean((sig(:)).^2);

x = sqrt(num/den);


function Y = imag2real(Y)
%IMAG2REAL

if isreal(Y)
    y = Y;
    return
end

realBase = real(Y.basis);
imagBase = imag(Y.basis);

lmi_variables = getvariables(Y);
nv = length(lmi_variables);

% [re im;-im re] = kron(I,re) + kron([0 1;-1 0],im)
sparse_X1 = [1 0;0 1];
sparse_X2 = [0 1;-1 0];
temp = kron(sparse_X1,reshape(realBase(:,1),Y.dim(1),Y.dim(1)))+ kron(sparse_X2,reshape(imagBase(:,1),Y.dim(1),Y.dim(1)));
temp = temp(:);

Y.basis = temp(:);
for i = 1:nv
    temp1 = kron(sparse_X1,reshape(realBase(:,i+1),Y.dim(1),Y.dim(1)));
    temp2 = kron(sparse_X2,reshape(imagBase(:,i+1),Y.dim(1),Y.dim(1)));
    Y.basis(:,i+1) = temp1(:) + temp2(:);
end;

Y.dim(1) = size(temp1,1);
Y.dim(2) = size(temp1,2);
Y = clean(Y);
% Reset info about conic terms
if isa(Y,'sdpvar')
    Y.conicinfo = [0 0];
end